Unmanned vehicle path planning method based on dynamic expansion guidance RRT algorithm

Abstract

The invention relates to an unmanned vehicle path planning method based on a dynamic expansion guidance RRT algorithm, and belongs to the technical field of intelligent driving path planning and autonomous navigation. According to the method, in a global path planning algorithm, firstly, an unmanned vehicle kinematics model is established by referring to vehicle incomplete kinematics constraints; then, proposing a dynamic expansion guide RRT algorithm which is low in RRT effective sampling efficiency and the like, and proposing a feasible region division strategy; providing a variable probability target guidance and vector superposition strategy for the problems of low path finding efficiency and the like caused by blindness of random tree expansion and lack of target guidance; for the problems that dense and useless expansion branches exist in a random tree structure, a planned path is tortuous and the like, a random tree sparsification and corner constraint strategy is provided so as to improve the utilization rate of the expansion tree branches and reduce the tortuosity of the path; and double father node reselection is provided to further optimize the path length, and key point extraction and path smoothing are utilized to obtain a global optimal path.

Description

Technical Field

[0001] The present invention belongs to the technical field of intelligent driving path planning and autonomous navigation, and specifically relates to a path planning method for an unmanned vehicle using a dynamic expansion guidance RRT algorithm. Background Art

[0002] In recent years, autonomous driving technology has rapidly developed, becoming a hot topic in the unmanned vehicle sector. The autonomous navigation capabilities of unmanned vehicles are crucial for achieving intelligent transportation, improving road safety, and increasing traffic efficiency. Intelligent vehicle technology encompasses a comprehensive suite of technologies encompassing perception, positioning, path planning, decision-making and control, and artificial intelligence, aiming to enable autonomous navigation, environmental awareness, and intelligent decision-making. By acquiring surrounding information through sensors, determining vehicle position, planning optimal routes, and leveraging decision-making systems to make intelligent driving decisions, intelligent vehicle technology is committed to improving traffic safety, efficiency, and comfort, driving innovation in future transportation.

[0003] Path planning is one of the key technologies for smart cars. Its goal is to enable smart cars to avoid static and dynamic obstacles and plan an optimal path (with the shortest time or shortest distance) when the starting point and destination are known. Path planning can be divided into global and local path planning. Global path planning refers to the process of finding the optimal path from the starting point to the destination within the entire map. This planning takes into account map information, road layout, and obstacles to determine the long-distance navigation of the vehicle in urban or complex environments. Local path planning is based on the environment around the vehicle's current position. Through real-time perception and obstacle avoidance, a safe and feasible path is selected to achieve precise navigation and obstacle avoidance operations in confined spaces. In practice, smart cars need to combine global and local path planning algorithms. The advantages and disadvantages of these two algorithms also determine the effectiveness of path planning. Summary of the Invention

[0004] The purpose of the present invention is to provide a path planning method for unmanned vehicles using a dynamic expansion-guided RRT algorithm to address the problems of low sampling efficiency, lack of goal orientation, and insufficient consideration of kinematic constraints in the current optimized RRT algorithm, which leads to tortuous paths. At the same time, the branch redundancy is dense, the utilization rate is low, and the generated path is non-optimal and has not undergone key point extraction and smoothing.

[0005] To achieve the above objectives, the technical solution of the present invention is: a method for unmanned vehicle path planning using a dynamic expansion guidance RRT algorithm, comprising:

[0006] Step 1: Initialize the parameters and read the coordinates of the current position of the unmanned vehicle, the position of the obstacle, and the position of the target point;

[0007] Step 2: Divide the sampling area;

[0008] Step 3: Through target guidance, vector superposition, branch sparseness and double parent node reselection strategy, the random tree is efficiently expanded towards the target;

[0009] Step 4: Through key point extraction and path smoothing, a simplified and smooth path that meets the steering constraints is obtained;

[0010] Step 5: The unmanned vehicle arrives at the destination, completes the path planning, and obtains the path planning route.

[0011] Furthermore, step one is specifically implemented as follows:

[0012] Obtaining environmental information: Obtaining the current position coordinates of the smart vehicle, the target point position coordinates, and the position coordinates of surrounding obstacles through on-board sensors;

[0013] Initialize vehicle parameters: Set the kinematic parameters of the intelligent vehicle, including maximum linear velocity, maximum angular velocity, minimum turning radius, and maximum steering angle;

[0014] Build a grid map: Convert environmental information into a grid map, mark the location of obstacles, feasible areas, and target points.

[0015] Furthermore, the on-board sensors include lidar and cameras.

[0016] Furthermore, in step 2, a sampling feasible domain partitioning strategy is proposed to divide the sampling area.

[0017] Furthermore, step 2 is specifically implemented as follows:

[0018] The entire environment map is binarized and the map environment is represented by matrix I1. The feasible region is marked as 1 and the infeasible region is marked as 0. To ensure the safety of the planned path, when selecting random points, not only the sampling points should not fall into the obstacle area, but also the predetermined distance from the obstacle should be maintained. The shortest distance away from the obstacle is the expansion distance d of the obstacle. s2 ;

[0019] Assume the length of the unmanned vehicle is a and the width of the unmanned vehicle is b. The minimum safe distance between the unmanned vehicle and the obstacle is also the obstacle expansion distance d. s2 for:

[0020]

[0021] Improve the safety of the planned path and avoid the danger caused by the planned path passing through a narrow road smaller than the vehicle width. Dense obstacles can be merged into the same obstacle. The judgment rule for obstacle merging is that if the distance between the obstacle surfaces is less than the set threshold, the obstacles will be merged. The judgment coefficient is d s1 Calculated as follows:

[0022] d s1 =k·b

[0023] Where k is the safety factor;

[0024] The map is pre-processed, including obstacle merging and expansion, and the sampling feasible region and infeasible region, namely the obstacle area, are divided.

[0025] Furthermore, in step three, the target guidance strategy adopts a variable probability target bias guidance strategy, which is expressed as follows:

[0026]

[0027] where q rand (x,y) is the coordinate of the random sampling point, q goal (x, y) is the coordinate of the target point, p rand is the random sampling probability, p bias is the target bias probability. When conducting target guidance, random sampling points are all taken from the end point q goal is the center of the circle, r d Select from the optimized circle domain of radius;

[0028]

[0029] p o is the probability change constant, k is the proportional coefficient, and d is the new node q of the random tree new and the end point q goal The distance between them, D is q rand With q goal The distance between collisionfree(q new ,q goal ) represents q new ,q goal There are no obstacles between them.

[0030] Furthermore, in step three, the vector superposition strategy is expressed as follows:

[0031]

[0032] in:

[0033]

[0034] where q new (x,y) is the new node q of the random tree new The x and y coordinates, q near (x,y) is a random sampling point q rand (x,y) coordinates of nearby points, e sp is the expansion step length,<A,B> Represents a vector |<A,B> | indicates the module length collisionfree(A,B) means there is no obstacle between points A and B, α and β represent the composite coefficients of the vector.<A,B> / |<A,B> |for The unit vector of the direction.

[0035] Furthermore, in step 3, the double parent node reselection strategy applies the triangle relationship theorem as follows:

[0036]

[0037] Where a, b, and c are the three sides of ΔABC; the three vertices of ΔABC represent the three nodes in the random tree. If the path is from point A to point C, there are two routes: A→C or A→B→C. According to the triangle relationship theorem, C A→B→C ≥C A→C Therefore, when this triangle relationship appears in the random tree, choosing A→C will further shorten the path and optimize the length of the path planning;

[0038] Get a new node q in the iteration new After obtaining a new node according to the RRT* algorithm, a preferred neighborhood circle will be established with the node as the center and r as the radius, and the nodes in the circle will be added to the set H as candidate nodes for the parent node of the new node. new The nearest node is q near ; Set q1 and q2 to q according to the RRT* algorithm new The parent node obtained from q new To q star The cost value is compared to obtain the optimal value q new The results show that q2 is the best choice when it is the parent node; further investigation is conducted on the parent nodes of the preferred neighborhood circle nodes, that is, investigations of q1, q2 and q near The parent node q of the node 1-f ,q 2-f and q near-f , the new node q new Directly connect to its parent node to get to the starting point q star The cost value is compared to obtain the optimal value, and the optimal value is the optimal parent node; when q new With q 2-f After connecting new ,q 2-f and q2 will form a triangle with nodes from q new to q 2-f According to the triangle relationship theorem, the distance Therefore, directly connect to its parent node q 2-f The optimal cost value will be obtained, and similarly, the comparison will continue with q1-f and q near-f When q is the parent node new to obtain the optimal value.

[0039] Furthermore, step four is implemented as follows:

[0040] The principle of extracting key point angle limitation is:

[0041]

[0042] Among them, P i is the node q of the planned path, G k is the kth key path point extracted, θ is the acute angle between any two adjacent paths after extraction, α max is the vehicle's front wheel limit steering angle, collisionfree (G k-1 ,G k ) indicates that the path between any two adjacent key nodes does not collide with obstacles;

[0043] The steps of extracting key points of the path are as follows: let the set of planned path points be {q i |i=1,2,3...n}, the set of extracted key points is {G k |k=0,1,2,...n}, first start from the starting node q1 and assign q1 to the key node G1, then connect q1 to q3 in sequence, and check whether the path of the q1q3 segment collides with the obstacle and whether the acute angle between q1q3 and q3q4 does not exceed the vehicle's limit steering angle α max If all the requirements are met, continue to connect to q1q4 and do the same check. If the requirements are met, continue to connect to q1q5, and so on, until q1q is connected j If the conditions are not met, then select connection q1q j-1 ,q j-1 For the key point G2, delete the intermediate redundant nodes q2 to q j-2 , update the path; then use q j-1 Repeat the above steps for the fixed point until the target point is reached; after key point extraction, redundant nodes are removed and the path length is optimized;

[0044] After the key points of the path are extracted, the path is a broken line. In order to ensure the smooth continuity of the path and better suit it for vehicle driving, it is necessary to perform cubic B-spline curve optimization on the segmented path between adjacent key points.

[0045] The present invention also provides a computer-readable storage medium on which computer program instructions that can be executed by a processor are stored. When the processor executes the computer program instructions, any of the method steps described above can be implemented.

[0046] Compared with the existing technology, the present invention has the following beneficial effects: to address the problem of low RRT effective sampling efficiency, the present invention proposes a feasible domain partitioning strategy; to address the problem of low pathfinding efficiency caused by the blindness of random tree expansion and lack of goal orientation, the present invention proposes a variable probability target guidance and vector superposition strategy; to address the problems of "dense and useless" expansion branches and tortuous planning paths in the random tree structure, the present invention proposes a random tree sparseness and corner constraint strategy to improve the utilization rate of expansion tree branches and reduce path tortuosity; and to propose a double parent node reselection to further optimize the path length and use key point extraction and path smoothing to obtain the global optimal path. BRIEF DESCRIPTION OF THE DRAWINGS

[0047] Figure 1 Obstacle preprocessing and obstacle safety zone division.

[0048] Figure 2 Obstacle handling steps.

[0049] Figure 3 Sampling simulation.

[0050] Figure 4 Target deviation diagram.

[0051] Figure 5 Environment - Simulation.

[0052] Figure 6 Environment 2 simulation.

[0053] Figure 7 Time comparison.

[0054] Figure 8 Comparison of the number of nodes.

[0055] Figure 9 Schematic diagram of the vector synthesis principle.

[0056] Figure 10 Environment 1.

[0057] Figure 11 Environment 2.

[0058] Figure 12 Schematic diagram of random tree sparsification.

[0059] Figure 13 Schematic diagram of random tree expansion branching angles.

[0060] Figure 14 Schematic diagram of random tree corner constraints.

[0061] Figure 15 Environment-path planning.

[0062] Figure 16 Environment II path planning.

[0063] Figure 17 Triangle Relation Theorem.

[0064] Figure 18 Step 1 of reselecting the second parent point.

[0065] Figure 19 Step 2 of reselecting the double parent node.

[0066] Figure 20 Double parent node simulation.

[0067] Figure 21 Schematic diagram of path key point extraction.

[0068] Figure 22 The angle between key points of the path.

[0069] Figure 23 Cubic spline path smoothing graph.

[0070] Figure 24 Environment-path fitting.

[0071] Figure 25 Environmental two-path fitting. DETAILED DESCRIPTION

[0072] The technical solution of the present invention will be described in detail below with reference to the accompanying drawings.

[0073] The present invention provides a method for unmanned vehicle path planning using a dynamic expansion guidance RRT algorithm, comprising:

[0074] Step 1: Initialize the parameters and read the coordinates of the current position of the unmanned vehicle, the position of the obstacle, and the position of the target point;

[0075] Step 2: Divide the sampling area;

[0076] Step 3: Through target guidance, vector superposition, branch sparseness and double parent node reselection strategy, the random tree is efficiently expanded towards the target;

[0077] Step 4: Through key point extraction and path smoothing, a simplified and smooth path that meets the steering constraints is obtained;

[0078] Step 5: The unmanned vehicle arrives at the destination, completes the path planning, and obtains the path planning route.

[0079] The following is a specific implementation process of the present invention.

[0080] The present invention provides a path planning method for an unmanned vehicle using a dynamic expansion guidance RRT algorithm, comprising the following steps:

[0081] Step 1: Initialize parameters and read the current coordinates of the unmanned vehicle, obstacle coordinates, and target point

[0082] Obtain environmental information: Obtain the current position coordinates of the smart vehicle, target point coordinates, and location information of surrounding obstacles through on-board sensors (such as lidar, cameras, etc.).

[0083] Initialize vehicle parameters: Set the kinematic parameters of the intelligent vehicle, including maximum linear velocity, maximum angular velocity, minimum turning radius, maximum steering angle, etc.

[0084] Build a grid map: Convert environmental information into a grid map, mark the location of obstacles, feasible areas, and target points.

[0085] Step 2: Strictly divide the sampling area to significantly improve sampling efficiency

[0086] The original RRT algorithm randomly selects sampling points in the entire state space, which will cause many sampling points to fall into the obstacle area. Therefore, in order to improve the effective sampling rate of the algorithm, it should be ensured that each sampling point falls in the feasible state domain. The original algorithm does not have a regional division strategy, so this section will propose an optimization for this. The entire environment map can be binarized and the map environment can be represented by matrix I1. For example, the feasible domain is marked as 1 and the infeasible domain is marked as 0. To ensure the safety of the planned path, when selecting random points, not only should the sampling points not fall into the obstacle area, but they must also maintain a certain distance from the obstacles. The shortest distance away from the obstacle is the obstacle expansion distance, as shown in the attached figure. Figure 1 d in (b) s2 .

[0087] Assume the length of the unmanned vehicle is a and the width of the unmanned vehicle is b. The minimum safe distance between the unmanned vehicle and the obstacle is also the obstacle expansion distance d. s2 for:

[0088]

[0089] Improve the safety of the planned path, avoid the planned path passing through narrow roads smaller than the vehicle width, and avoid the danger caused by the narrow roads. For dense obstacles, they can be combined into the same obstacle. Figure 1 As shown in (a), the obstacle merging rule is that if the distance between the obstacle surfaces is less than the set threshold, the obstacles will be merged, and the determination coefficient is d s1 Calculated as follows:

[0090] d s1 =k·b

[0091] Where k is the safety factor;

[0092] The map preprocessing is as follows Figure 2 As shown in the figure, dividing the sampling feasible region and obstacle area after obstacle merging and expansion can improve the sampling efficiency and the expansion speed and quality of the random tree.

[0093] A simulation experiment was conducted on this sampling strategy. If the sampling points are uniformly sampled in the entire space, the effective sampling rate in dividing the sampling feasible region and the infeasible region (i.e., the obstacle region) is:

[0094]

[0095] Where S is the area of the entire state space, i.e. the map area, S f =SS O is the free area, S O is the area of the obstacle domain.

[0096] Attachment Figure 3 The sampling simulation diagram is shown in Figure 2. The red points in the environment are sampling points, the black squares are obstacles, and the blue areas are expansion areas. Figure 3 In (a), we can see that there are densely packed red sampling points on the map, and quite a few of them fall into the black and blue areas. The sampling points that fall into these areas are invalid points. In order to improve the safety of path planning, dense obstacles are merged. Figure 3 (b) To merge and expand dense obstacles. After preprocessing the feasible domain of obstacles, sampling points in the feasible domain can improve sampling efficiency. Figure 3 In (c), none of the red points fall into the obstacle area or the collision area, and all sampling points are in the feasible region, which will improve the efficiency of random tree sampling expansion.

[0097] Step 3: Through target guidance, vector superposition, branch sparseness and double parent node reselection strategy, the random tree is efficiently expanded towards the target;

[0098] The RRT algorithm is a probabilistically complete algorithm that can find a feasible path from the starting point to the end point within a sufficient runtime. However, the random nature of sampling points leads to blind expansion of the random tree, which reduces the algorithm's path planning efficiency. To improve the existing RRT algorithm, some scholars have proposed a target-guided strategy, which directs the random tree growth toward a given target point. Therefore, to improve the effective expansion efficiency of the random tree and enable it to grow toward the target more quickly, several improvement strategies are proposed below: a target-guided strategy, a new branch expansion strategy using vector superposition, a branch sparseness strategy, and a dual-parent node reselection strategy for improved path optimization. The principles of these improvement strategies will be explained in turn, followed by simulation experiments.

[0099] Because the original RRT algorithm uses uniform random sampling, which results in blind tree growth and low pathfinding efficiency, a variable probability target guidance strategy is proposed. To enable the random tree to grow toward the endpoint more quickly, it is envisioned that different target bias probabilities are applied during different expansion periods of the random tree, thereby improving the efficiency of the random tree's expansion toward the endpoint.

[0100] If a random tree grows blindly without guidance during its expansion, it is like walking at night. Regarding target guidance, some scholars have proposed a target bias guidance strategy, that is, setting a probability threshold. The algorithm has a probability to use the target point as the sampling point to make the random tree expand directly to the end point, thereby accelerating the growth of the random tree. The principle is as follows:

[0101]

[0102] where q rand (x,y) is the x and y coordinates of the random sampling point, q goal (x,y) is the x and y coordinates of the target point, p rand is the sampling probability value of the random sampling point, p bias is the set bias probability value.

[0103] There is a certain probability that the target point can be directly used as a random point when sampling the target bias, but the target bias threshold p set in this way is bias Fixed, unable to change with the changes in the environment, and poor adaptability. Inspired by this, the present invention proposes a variable probability target bias guidance strategy. The random tree expansion process can be simply divided into two stages, the main purpose of the random tree in the front stage is search expansion, and the latter stage is path convergence. In order to ensure that the random tree accelerates multi-directional expansion in the front stage to avoid falling into local difficulties and causing path planning failure. Set a sampling optimization area in the map, and further restrict the sampling area of the random point at a specific time. This can speed up the expansion of the random tree toward the target. In the present invention, the sampling optimization area is based on the end point q goal is the center of the circle, r d The radius of the optimized circle is established. When k appears in the circle c The node triggers the expansion guidance strategy as shown in the attached Figure 4 .

[0104]

[0105] Where ε is the radius scaling factor, L is the horizontal map length, W is the vertical map width, and dist(q star ,q goal ) is the distance between the starting point and the end point, γ is the distance coefficient, assuming there are n obstacles, O Li is the maximum horizontal length of obstacle i, O Wi is the maximum longitudinal length of obstacle i, k OL is the maximum horizontal ratio of all obstacles. Similarly, k OW It is the maximum longitudinal ratio of all obstacles.

[0106] If the sampling of the RRT algorithm is uniform random sampling, the number of random tree nodes can be simplified to the number of nodes per unit area. Assume that there can only be k1 nodes in the same sparse circle, and the radius of the sparse circle is r.c Then the estimated number of nodes is:

[0107]

[0108] Where L is the map length, W is the map width, r c is the radius of the rarefaction circle.

[0109] The number of nodes is recorded during the random tree expansion process. When the number of nodes reaches more than half of the estimated number or when there are k c When a node appears in the sampling optimization area, the random tree will enter the expansion guidance, that is, the sampling area is limited to the optimization circle area. The principle is as follows:

[0110]

[0111] In the formula is a random sampling point q rand The x and y coordinates of r d The radius of the sampling optimization circle, p rand The random number can also be called a random point sampling probability is a random number between (0,1), M one The matrix after sampling the feasible region for the map environment.

[0112] When the algorithm enters the optimization sampling, the sampling point will have a probability of reaching the end point q goal , that is: q rand =q goal In order to better adapt to environmental changes, the probability of taking the end point as the sampling point is changed as follows:

[0113]

[0114] where q rand (x,y) is the coordinate of the random sampling point, q goal (x, y) is the coordinate of the target point, p rand is the random sampling probability, p bias is the target bias probability. When conducting target guidance, random sampling points are all taken from the end point q goal is the center of the circle, r d Select from the optimized circle domain of radius;

[0115]

[0116] p o is the probability change constant, k is the proportional coefficient, and d is the new node q of the random tree new and the end point q goal The distance between them, D is q rand With q goal The distance between collisionfree(qnew ,q goal ) represents q new ,q goal There are no obstacles between them.

[0117] So in the random tree expansion, if the number of nodes reaches more than half of the estimated total number or there are k c When the nodes fall into the sampling optimization circle, the algorithm will start to guide the target point. When the target guidance starts in a certain iteration, the random points collected are all in the optimization circle. There are p sampling points selected this time. bias The probability of getting the target point. Assume that the sampling point selected this time is q i , the new node added to the random tree T is q new , calculating the probability of the next target deviation. That is, if there are no obstacles between the new node and the target point, the target deviation probability is increased, accelerating the growth toward the target point. If there are obstacles, the initial target deviation probability is restored. By optimizing the sampling circle and guiding the target deviation, the expansion and convergence of the random tree are improved, accelerating the path planning process.

[0118] In order to compare the advantages and disadvantages of the target bias guidance strategy and the variable probability target guidance RRT algorithm, two environments were set up for simulation experiments. The state space size is 600×600 unit length, the step length is 20 unit length, the starting point is the pink point, and the end point is the green point, with coordinates of (100,550) and (500,50) respectively. The blue solid line in the figure is the random expansion tree.

[0119] Attachment Figure 5 and attached Figure 6 The following is a simulation comparison of target bias guidance and the variable probability target guidance strategy proposed by the present invention under different environments. Figure 5 It can be seen that compared with the variable probability target bias guidance, the random tree has more expansion branches and expansion nodes; secondly, the variable probability target guidance random tree expansion branch growth has a strong directionality, which can make the random tree reach the end point faster. Figure 6 For the simulation experiment of environment 2, (100,50) is set as the starting point and (530,500) is set as the end point. The target bias strategy algorithm expands branches throughout the environment space, while the random tree of the variable probability target guidance strategy can bypass complex obstacles, has strong target guidance, and accelerates the end point orientation.

[0120] Since the fast search random tree algorithm has randomness, in order to further prove the feasibility of the improvement proposed in this section, multiple repeated experiments are conducted for target bias and variable probability target guidance. Figure 7The time comparison chart shows that in environment 1, the average search time for target-directed search was 2.75s, and the average search time for variable probability guidance was 1.32s, which was a 52% reduction in search time. In environment 2, the average search time for target guidance and variable probability guidance was 2.93s and 1.42s, respectively, a 51% reduction in search time. Figure 8 The following figure compares the number of path nodes. It shows that in both environments one and two, the average number of nodes for target-directed guidance is greater than that for variable probability guidance. In environment one, the average number of nodes for target-directed guidance is 281, while for variable probability guidance it is 130, a 53% reduction. In environment two, the average number of nodes for target-directed guidance is 319, while for variable probability guidance it is 157, a 50% reduction. Analysis of the time-to-node comparison line graph and planning simulation graphs demonstrates that variable probability guidance exhibits greater adaptability than traditional target-directed guidance in different environments, significantly reducing both the number of pathfinding nodes and the time required.

[0121] During a random tree expansion, if the direction of the branch extension is far from the target point, the tree convergence speed will be slowed down, and the path planning efficiency will be reduced. Therefore, this paper proposes a new branch expansion strategy using vector superposition to further improve the endpoint orientation of the random tree. This principle will be elaborated in detail later, and simulation experiments will be conducted to verify the feasibility of this strategy.

[0122] The principle of vector synthesis is used to speed up the growth of the random tree towards the target point, so that the azimuth angle of the random tree expansion depends not only on the sampling point but also on the target point. The principle diagram is shown in the attached figure. Figure 9 As shown. At a certain iteration, the algorithm obtains the white sampling point q rand , and retrieve the random tree to get the nearest node q near , if the original RRT algorithm steps should be towards Direction expansion step length e sp , get the new node q of the random tree new But the vector superposition strategy is: the random tree expansion direction obtained this time is It is far away from the target point, and the random tree will not accelerate the convergence when expanding in this direction. vector to enhance target orientation.

[0123] The subsequent discussion of random tree expansion direction will be divided into different situations, as shown in the attached Figure 9 (a) If the random point q obtained this time rand The nearest node is q near , then we need to detect the nearest node q near With q goal Is there an obstacle between There are no obstacles between Figure 9 As shown in (a)-(b), the expansion direction of the random tree is That is, expand the step length e directly towards the target point sp , get the new node q new The green node is shown in the figure. Otherwise There are obstacles in the middle such as Figure 9 (c) The expansion direction of the random tree uses the vector synthesis principle, and the expansion direction is given by and Joint decision, as attached Figure 9 (d) shows that The angle of the direction deviating from the target point is large, and the expanded direction after synthesis The angle of deviation from the target point is small, and then the step length is expanded in that direction to obtain the new node q new , as attached Figure 9 (d) Green node. This can accelerate the random tree towards the target point and improve the path planning speed. The specific principle is:

[0124]

[0125] in:

[0126]

[0127] where q new (x,y) is the x and y coordinates of the newly added node, q near (x,y) is the coordinate of the adjacent point, e sp is the expansion step length,<A,B> Represents a vector |<A,B> | indicates the module length collisionfree(A,B) means there is no obstacle between points A and B, α and β represent the composite coefficients of the vector.<A,B> / |<A,B> |for The unit vector of the direction.

[0128] Attachment Figure 10 For the environment 1 simulation experiment, set (100,550) as the starting point and (500,50) as the end point. Figure 10 (a) is the original expansion method. It can be seen that the random tree expansion branches are disorderly, and many branches deviate from the end position. Figure 10 In (b), vector synthesis can reduce the deviation of the expansion branch from the target point, so that the random tree expansion is directed towards the end point. Figure 11 This is the simulation experiment for environment 2, with (100,50) as the starting point and (530,500) as the end point. It can be seen from the figure that vector superposition expansion has a stronger expansion purpose than the original expansion method; secondly, the number of random tree nodes is significantly smaller, which can improve the algorithm's pathfinding efficiency.

[0129] As previously mentioned, simulation experiments with RRT and its derivative algorithms have shown that the numerous random tree branches during pathfinding affect the algorithm's efficiency. While the RRT* algorithm has optimized some branches through parent node reselection and random reselection strategies, many unused branches still affect path planning efficiency. Secondly, the random tree branch expansion angle affects the smoothness of autonomous vehicle driving. If the tortuous branches do not meet the steering requirements of the autonomous vehicle, practical application will be difficult. Therefore, the branch expansion angle is restricted, constraining the branch expansion angle to meet the vehicle's steering requirements. Subsequently, the random tree branches will be sparsely simplified and angle constraints will be added to meet the autonomous vehicle's driving requirements.

[0130] As previously mentioned, simulation analysis shows that the RRT algorithm, without pruning and thinning, has numerous branches, many of which overlap, intersect, and are useless. These branches affect algorithm efficiency and are detrimental to pathfinding. Therefore, pruning and thinning the random tree are essential for improving the RRT algorithm. The core idea is to assign a sequence number and parent node sequence number to each new node generated during each iteration of the algorithm and add it to the random tree T.

[0131] After the algorithm generates a new node through a certain iteration through a nearby point, a sparse circle is constructed with the new node as the center and a certain length as the radius. If there are no other existing nodes in the circle, they are directly added to the random tree T. If there are existing nodes in the circle, they are checked to see if they belong to the same parent node. If they do, their costs are compared. If the cost of the new node is better than that of the node with the same parent, the other node is discarded and the new node is added. Otherwise, the new node is discarded. If the node in the circle and the new node do not have the same parent, their costs are compared. If the cost is better than the new node, the new node is discarded. Otherwise, the new node is retained.

[0132] Attachment Figure 12 The figure shows a schematic diagram of random tree sparseness, where the white q new The node is a new node obtained in each iteration, and then it is used as the center of the circle, r sp Make a sparse circle with radius . Figure 12 As shown in (a), when there are no other nodes in the sparse circle, the new node is directly included in the random tree T; Figure 12 (b) There are other nodes in the sparse circle and q r With q new If they have the same parent node, calculate their cost value. Obviously, if the cost value of the new node is lower, then discard the yellow q r Node and its branches; Figure 12 (c) shows that when there are more than new nodes q in the sparse circle new The node with better cost value will abandon the new node of this iteration.

[0133] Random tree sparse simplification and region partitioning can improve algorithm efficiency to a certain extent. However, without angle constraints on branches, the path will be tortuous. Specifically, the difference between the branch angle of the newly expanded branch and the parent node will be too large, causing the steering angle to exceed the maximum turning angle of the vehicle. Therefore, the random tree expansion process must not only ensure high efficiency but also add steering constraints. The RRT algorithm is extended with angle constraints. The principle is to establish a rear axle center angle formula based on the bicycle model based on the vehicle's current posture and the next trajectory point, thereby controlling the vehicle to travel around the circular curve to the next point.

[0134] As attached Figure 13 Figure (a) is a schematic diagram of random tree expansion, Figure 13 (b) is a schematic diagram of the steering angle, assuming that the current vehicle position is q near , the preview point is q new , the current vehicle posture and the preview point q new The angle between the two is α, and the distance from the preview point is e sp , the turning radius is R. According to the schematic diagram, ΔO′q new q new-f , according to the geometric relationship:

[0135] e sp cos(α)=Rsin(2α)

[0136] After sorting, we have:

[0137]

[0138] We can get:

[0139]

[0140] By the attached Figure 13 It can be seen that the RRT algorithm expansion branch connects the adjacent point q through random sampling points near Expand outward, and the smoothness of the path is mainly related to the connection of the nodes. Combining the mathematical principles explained above to constrain the RRT random tree expansion angle can generate a path that conforms to the vehicle turning, and the new random tree node q can be obtained new Its neighboring node q near The angle constraint of the front wheel is δ f The relationship with the angle α is:

[0141]

[0142] where e sp is the random tree expansion step size, L is the vehicle wheelbase, and α is the angle between the current vehicle direction angle and the next direction angle, which is also the angle between the random tree branches.

[0143] According to the literature, the limit turning angle δ of the unmanned vehicle isfmax Usually it is between 30° and 40°. For the same unmanned vehicle, its wheelbase is a constant value. Then the node q near Posture and new node q new The line angle α and the random tree expansion step e sp The change relationship is as shown above. In order to make the path smoother and the transition between nodes smoother, the maximum value of the angle α is max The front wheel limit turning angle δ must not be exceeded fmax , where the front wheel limit turning angle δ fmax Take 30° and then calculate the random tree expansion step size according to formula 3-15. Therefore, when selecting adjacent points for random tree expansion, not only the expansion step size should be considered but also the rotation angle constraint should be added to add angular orientation information to the expansion node to overcome the defect of excessively large angles between adjacent branches of the random tree.

[0144] Attachment Figure 14 (a) is a schematic diagram of the corner in the random tree expansion, Figure 14 (b) is the calculation diagram of the corner constraint. It can be seen from the figure that And θ≤α max After a new node is obtained in the random tree, the angle difference between the newly expanded branch and the adjacent branch needs to be calculated. The formula is as follows, and the cosine value of the angle θ is recorded as k:

[0145]

[0146] in q near Point to q new vector, is a vector The module length of Meaning the same.

[0147] The above formula can be sorted out to get:

[0148] 0≤θ=arcsin(k)≤α max

[0149] During a certain iteration of the algorithm, a new node q is obtained new Calculate later and If the angle is too large and exceeds the vehicle's front wheel turning angle limit δ fmax The latest node obtained in this iteration is invalid and the next iteration is entered. If the value of θ does not exceed the limit steering angle δ fmax , then the expanded node is valid and is incorporated into the random tree T. θ is calculated as shown above. The corner constraint keeps the random tree branch angle within a reasonable range, ensuring a smooth transition when the autonomous vehicle follows the path and meets the vehicle cornering requirements.

[0150] From the simulation experiment, it can be concluded that the improved random tree expansion has a strong sense of purpose and orientation, but the random tree has many branches in space, which will lead to a large number of useless branches and affect the efficiency of the algorithm. In order to show the situation of adding vehicle corner constraints and sparse optimization to the algorithm, simulation comparison experiments were carried out using the original algorithm without corner constraints and sparse optimization and the RRT algorithm with corner constraints and sparse optimization. The simulation environment settings and the starting and ending points are the same as above, and environment 1 and environment 2 are still selected. Figure 15 and attached Figure 16 The figure is a comparison of experimental results, in which the red ones are feasible paths found. Figure 15 The planned path in (a) is tortuous, with many random branches and dense branches; Figure 15 (b) By comparison, it can be concluded that the path planned by the algorithm is significantly smoother after adding sparse and steering constraints, and the tortuosity is significantly improved, which is more in line with vehicle driving requirements.

[0151] Attachment Figure 16 The pink starting point is (100,50) and the green ending point is (530,500). There are many overlapping random tree branches in the figure. After introducing sparseness, as shown in the attached figure, Figure 16 (b) The random tree is obviously sparse, the invalid branches of expansion are reduced, and the utilization rate of expansion branches is improved. It can be seen that the planning path is smoother and the number of random tree nodes is significantly reduced.

[0152] The RRT* algorithm is a derivative of the RRT algorithm. Its changes compared to the RRT algorithm can be simply summarized into two points: parent node reselection and random reconnection. Parent node reselection optimizes random sampling points to select the nearest point as the parent node, resulting in a new node to the starting point q star The cost value of the new node is not optimal. After reselecting the parent node, the cost value of the new node can be optimized. Random reconnection ensures that the new node q new The cost value of the nodes in the neighborhood of the circle center is optimal.

[0153] The double parent node reselection strategy is similar to the parent node reselection strategy of the RRT* algorithm. The double parent node reselection strategy applies the triangle relationship theorem as follows:

[0154]

[0155] Where a, b, and c are the three sides of ΔABC.

[0156] The three vertices of ΔABC represent the three nodes in the random tree, represented by Figure 17 It can be seen that if the path is from point A to point C, there are two routes: A→C or A→B→C. According to the triangle relationship theorem, C A→B→C ≥C A→C Therefore, when this triangle relationship appears in the random tree, if A→C can be selected, the path can be further shortened and the length of the path planning can be optimized.

[0157] In the iteration, the Figure 18 The white new node q shown in (a) new After obtaining a new node according to the RRT* algorithm, a preferred neighborhood circle will be established with the node as the center and r as the radius, and the nodes in the circle will be added to the set H as candidate nodes for the parent node of the new node. new The nearest node is q near According to the RRT* algorithm, q1 and q2 are set to q new The parent node obtained from q new To q star The cost value is compared to obtain the optimal value as its parent node. The result shows that q2 is the optimal choice when it is the parent node. Figure 18 (c) shows the optimization strategy of the RRT* algorithm. In the present invention, further optimization strategies will be proposed.

[0158] When the optimization results of the RRT* algorithm are as shown in the attached Figure 19 As shown in (a), the optimization strategy of this section will further examine the parent node of the preferred neighborhood circle node, as shown in the attached Figure 19 (a) shows that q1, q2 and q are examined separately near The parent node q of the node 1-f ,q 2-f and q near-f , when the new node q new The point q obtained by directly connecting its parent node star The cost value is compared to obtain the optimal value, and the optimal value is the optimal parent node. new With q 2-f After connecting new ,q 2-f and q2 will form a triangle with nodes from q new to q 2-f According to the triangle relationship theorem, the distance Therefore, directly connect to its parent node q 2-f The optimal cost value will be obtained. This is the basic principle of the double parent node. Similarly, we will continue to compare 1-f and q near-f When q is the parent node new The cost value of is used to obtain the optimal value. The double parent node optimization strategy can further improve the path optimization quality.

[0159] Attachment Figure 20 This is a simulation experiment of adding a double parent node strategy to the algorithm. The blue lines in the figure are the optimized random tree expansion branches, the green lines are the branches after adding the double parent node strategy, and the red thick solid line is the feasible path found. Figure 20(a) In environment 1, the pink starting point is (100,550) and the green end point is (500,50). Figure 20 (b) The starting and ending points of environment 2 are (100, 50) and (530, 500) respectively. To enhance the comparison, the branches of the improved algorithm are drawn. From the simulation diagrams of the two, it can be seen that after the double parent node optimization, the green random tree branches are straighter, which can reduce the path tortuosity. Figure 20 It is not difficult to see from (a) and (b) that by adding the double-parent node strategy, the planned path length is significantly reduced compared to the original one, which can maximize the shortest path while reasonably avoiding obstacles.

[0160] Step 4: Through key point extraction and path smoothing, a simplified and smooth path that complies with the steering constraints is obtained;

[0161] Because the RRT algorithm obtains feasible paths through random tree expansion, its inherent properties can lead to tortuosity in the planned path. The improvements described above have significantly improved the algorithm's efficiency, path feasibility, and smoothness. However, the path still contains a large number of inflection points. Simulation analysis shows that the path tortuosity increases with more complex environments and more obstacles. Therefore, in this section, key point extraction and path smoothing will be performed to simplify the path while meeting the steering constraints of the autonomous vehicle.

[0162] Path key point extraction is of great significance for path simplification. On the one hand, it can simplify redundant nodes, and on the other hand, it can improve path quality. Figure 21 Figure (a) shows the planned path. It can be seen from the figure that the path is relatively smooth but there are still many turning points. Therefore, the redundant nodes can be appropriately simplified to improve the straightness of the path planning. Figure 21 (b) is a schematic diagram of key point extraction. It can be seen from the figure that if the starting point q is directly star With q i+1 Connecting them will remove the q in the path i and its previous redundant nodes to improve path straightness and obtain a shorter path.

[0163] The diagram of angle restriction for extracting key points is shown in the attached figure. Figure 22 As shown, the principle is:

[0164]

[0165] Among them, P i is the node q of the planned path, G k is the key path point extracted, θ is the acute angle between any two adjacent paths after extraction, and α max is the vehicle's front wheel limit steering angle, collisionfree (G k-1 ,G k) indicates that the path between any two adjacent key nodes does not collide with obstacles.

[0166] The steps of extracting key points of the path are as follows: let the set of planned path points be {q i |i=1,2,3...n}, the set of extracted key points is {G k |k=0,1,2,...n}, first start from the starting node q1 and assign q1 to the key node G1, then connect q1 to q3 in sequence, and check whether the path of the q1q3 segment collides with the obstacle and whether the acute angle between q1q3 and q3q4 does not exceed the vehicle's limit steering angle α max If all the requirements are met, continue to connect to q1q4 and do the same check. If the requirements are met, continue to connect to q1q5, and so on, until q1q is connected j If the conditions are not met, then select connection q1q j-1 ,q j-1 For the key point G2, delete the intermediate redundant nodes q2 to q j-2 , update the path; then use q j-1 Repeat the above steps for each fixed point until the target point is reached. After key point extraction, redundant nodes are removed and the path length is optimized.

[0167] After keypoint extraction, the path is a broken line. To ensure smooth continuity for vehicle operation, the segmented path between adjacent keypoints requires curve smoothing. Common curve fitting methods include least squares, Bezier curves, and B-spline curves. B-spline curves are an improvement on Bezier curves, offering superior performance by overcoming their limitations in local adjustments. Furthermore, B-spline curves offer advantages such as curvature continuity, making B-spline curve optimization widely used in trajectory and path planning.

[0168] For P0, P1, P2...P n The k-order B-spline curve formula of the control vertex is:

[0169]

[0170] Among them, the i-th k-order B-spline basis function is B i,k (u) is, and P i Correspondingly, k≥1 in the formula.

[0171] The de Boer-Cox recursion of the B-spline basis function is:

[0172]

[0173] It is agreed that 0 / 0=0, u i Is the node vector, when determining B in the curve i,k(u) needs to be used i 、u i+1 ...u i+k There are k+1 nodes in total.

[0174] The smooth path of the B-spline curve needs to pass through the given key points, that is, the key points are used as the type value points, so the control points need to be solved through the type value points. i (i=0,1,2...n) passes, the key path point currently extracted is G i (i=1,2,3...n) Solve. The relationship between the type value point and the control point is:

[0175]

[0176] At the same time, the curve passes through the starting point and the end point, and P1=G1, P n =G n In addition, in order to agree on the boundary conditions, the curve is set to be P0P1 and P0P1 at the starting point and end point respectively. n P n-1 Tangent. Figure 23 This is a simulation diagram of path smoothing using cubic B-spline curve.

[0177] Step 5: The unmanned vehicle arrives at the destination, completes the path planning, and obtains the path planning route

[0178] At this point, the smart car has completed the global path and can reach the destination after certain calculations. After the above multi-strategy fusion improvements, the optimized RRT algorithm can plan a smoother path, while avoiding obstacles reasonably and expanding faster. Vehicle steering constraints have been added to the path, but the path smoothness needs to be improved, which is not conducive to vehicle tracking control. Therefore, in order to make the path smoother and meet the vehicle motion control, the key points of the planned path are extracted, and then the path is optimized using a cubic B-spline curve. In order to compare the difference between the paths before and after fitting, key point extraction and path fitting are performed on the feasible paths planned for environment one and two, as shown in the attached figure. Figure 24 and attached Figure 25 .

[0179] Attachment Figure 24 For key point extraction and fitting in environment 1, Figure 24 The red thick solid line in (a) is the planned feasible path, and the black thick dots in the figure are the key points extracted from the path. It can be seen that the optimized red path is relatively smooth, with no sharp points or sharp turning points in the path, but the path is not a smooth path. Figure 24 The dark green path in b) is the path fitted based on the key points. It can be seen that the dark green path not only satisfies reasonable obstacle avoidance but also has a smooth curve, which is conducive to vehicle motion control.

[0180] Attachment Figure 25 This is the key point extraction and fitting of the second environment path. As can be seen from Figure (a), the extracted black key points can best reflect the obstacle avoidance and steering in the path. The dark green path in Figure (b) is fitted based on the key points. Under the constraints of the key points, it reasonably avoids obstacles and meets the vehicle's driving smoothness. The fitted path is safer and more reasonable.

[0181] The present invention also provides a computer-readable storage medium on which computer program instructions that can be executed by a processor are stored. When the processor executes the computer program instructions, any of the method steps described above can be implemented.

[0182] The above are preferred embodiments of the present invention. Any changes made according to the technical solution of the present invention, as long as the resulting functions and effects do not exceed the scope of the technical solution of the present invention, shall fall within the scope of protection of the present invention.

Claims

1. A dynamic expansion guided RRT algorithm unmanned vehicle path planning method, characterized by: include: Step 1: Initialize the parameters and read the coordinates of the current position of the unmanned vehicle, the position of the obstacle, and the position of the target point; Step 2: Divide the sampling area; Step 3: Through target guidance, vector superposition, branch sparseness and double parent node reselection strategy, the random tree is efficiently expanded towards the target; Step 4: Through key point extraction and path smoothing, a simplified and smooth path that meets the steering constraints is obtained; Step 5: The unmanned vehicle arrives at the destination, completes the path planning, and obtains the path planning route.

2. The method for unmanned vehicle path planning using a dynamic expansion guidance RRT algorithm according to claim 1, characterized in that: Step 1 is implemented as follows: Obtaining environmental information: Obtaining the current position coordinates of the smart vehicle, the target point position coordinates, and the position coordinates of surrounding obstacles through on-board sensors; Initialize vehicle parameters: Set the kinematic parameters of the intelligent vehicle, including maximum linear velocity, maximum angular velocity, minimum turning radius, and maximum steering angle; Build a grid map: Convert environmental information into a grid map, mark the location of obstacles, feasible areas, and target points.

3. The method for unmanned vehicle path planning using a dynamic expansion guidance RRT algorithm according to claim 2 is characterized in that: On-board sensors include lidar and cameras.

4. The method for unmanned vehicle path planning using a dynamic expansion guidance RRT algorithm according to claim 1, characterized in that: In step 2, a sampling feasible domain partitioning strategy is proposed to divide the sampling area.

5. The method for unmanned vehicle path planning using a dynamic expansion guidance RRT algorithm according to claim 4, characterized in that: Step 2 is implemented as follows: The entire environment map is binarized and the map environment is represented by matrix I1. The feasible region is marked as 1 and the infeasible region is marked as 0. To ensure the safety of the planned path, when selecting random points, not only the sampling points should not fall into the obstacle area, but also the predetermined distance from the obstacle should be maintained. The shortest distance away from the obstacle is the expansion distance d of the obstacle. s2 ; Assume the length of the unmanned vehicle is a and the width of the unmanned vehicle is b. The minimum safe distance between the unmanned vehicle and the obstacle is also the obstacle expansion distance d. s2 for: Improve the safety of the planned path and avoid the danger caused by the planned path passing through a narrow road smaller than the vehicle width. Dense obstacles can be merged into the same obstacle. The judgment rule for obstacle merging is that if the distance between the obstacle surfaces is less than the set threshold, the obstacles will be merged. The judgment coefficient is d s1 Calculated as follows: d s1 =k·b Where k is the safety factor; The map is pre-processed, including obstacle merging and expansion, and the sampling feasible region and infeasible region, namely the obstacle area, are divided.

6. The method for unmanned vehicle path planning using a dynamic expansion guidance RRT algorithm according to claim 1, characterized in that: In step three, the target guidance strategy adopts the variable probability target bias guidance strategy, which is expressed as follows: where q rand (x,y) is the coordinate of the random sampling point, q goal (x, y) is the coordinate of the target point, p rand is the random sampling probability, p bias is the target bias probability. When conducting target guidance, random sampling points are all taken from the end point q goal is the center of the circle, r d Select from the optimized circle domain of radius; p o is the probability change constant, k is the proportional coefficient, and d is the new node q of the random tree new and the end point q goal The distance between them, D is q rand With q goal The distance between collisionfree(q new ,q goal ) represents q new ,q goal There are no obstacles between them.

7. The method for unmanned vehicle path planning using a dynamic expansion guidance RRT algorithm according to claim 6, characterized in that: In step three, the vector superposition strategy is expressed as follows: in: where q new (x,y) is the new node q of the random tree new The x and y coordinates, q near (x,y) is a random sampling point q rand (x,y) coordinates of nearby points, e sp is the expansion step length,<A,B> Represents a vector |<A,B> | indicates the module length collisionfree(A,B) means there is no obstacle between points A and B, α and β represent the composite coefficients of the vector.<A,B> / |<A,B> |for The unit vector of the direction.

8. The method for unmanned vehicle path planning using a dynamic expansion guidance RRT algorithm according to claim 1, characterized in that: In step 3, the double parent node reselection strategy applies the triangle relationship theorem as follows: Where a, b, and c are the three sides of ΔABC; the three vertices of ΔABC represent the three nodes in the random tree. If the path is from point A to point C, there are two routes: A→C or A→B→C. According to the triangle relationship theorem, C A→B→C ≥C A→C Therefore, when this triangle relationship appears in the random tree, choosing A→C will further shorten the path and optimize the length of the path planning; Get a new node q in the iteration new After obtaining a new node according to the RRT* algorithm, a preferred neighborhood circle will be established with the node as the center and r as the radius, and the nodes in the circle will be added to the set H as candidate nodes for the parent node of the new node. new The nearest node is q near ; Set q1 and q2 to q according to the RRT* algorithm new The parent node obtained from q new To q star The cost value is compared to obtain the optimal value q new The results show that q2 is the best choice when it is the parent node; further investigation is conducted on the parent nodes of the preferred neighborhood circle nodes, that is, investigations of q1, q2 and q near The parent node q of the node 1-f ,q 2-f and q near-f , the new node q new Directly connect to its parent node to get to the starting point q star The cost value is compared to obtain the optimal value, and the optimal value is the optimal parent node; when q new With q 2-f After connecting new ,q 2-f and q2 will form a triangle with nodes from q new to q 2-f According to the triangle relationship theorem, the distance Therefore, directly connect to its parent node q 2-f The optimal cost value will be obtained, and similarly, the comparison will continue with q 1-f and q near-f When q is the parent node new to obtain the optimal value.

9. The method for unmanned vehicle path planning using a dynamic expansion guidance RRT algorithm according to claim 1, characterized in that: Step 4 is implemented as follows: The principle of extracting key point angle limitation is: Among them, P i is the node q of the planned path, G k is the kth key path point extracted, θ is the acute angle between any two adjacent paths after extraction, α max is the vehicle's front wheel limit steering angle, collisionfree (G k-1 ,G k ) indicates that the path between any two adjacent key nodes does not collide with obstacles; The steps of extracting key points of the path are as follows: let the set of planned path points be {q i |i=1,2,3...n}, the set of extracted key points is {G k |k=0,1,2,...n}, first start from the starting node q1 and assign q1 to the key node G1, then connect q1 to q3 in sequence, and check whether the path of the q1q3 segment collides with the obstacle and whether the acute angle between q1q3 and q3q4 does not exceed the vehicle's limit steering angle α max If all the requirements are met, continue to connect to q1q4 and do the same check. If the requirements are met, continue to connect to q1q5, and so on, until q1q is connected j If the conditions are not met, then select connection q1q j-1 ,q j-1 For the key point G2, delete the intermediate redundant nodes q2 to q j-2 , update the path; then use q j-1 Repeat the above steps for the fixed point until the target point is reached; after key point extraction, redundant nodes are removed and the path length is optimized; After the key points of the path are extracted, the path is a broken line. In order to ensure the smooth continuity of the path and better suit it for vehicle driving, it is necessary to perform cubic B-spline curve optimization on the segmented path between adjacent key points.

10. A computer-readable storage medium storing computer program instructions that can be executed by a processor, wherein when the processor executes the computer program instructions, the method steps according to any one of claims 1 to 9 can be implemented.

Images