CN117892896B - Dynamic path planning method based on improved bidirectional RRT algorithm - Google Patents
Dynamic path planning method based on improved bidirectional RRT algorithm Download PDFInfo
- Publication number
- CN117892896B CN117892896B CN202410118463.1A CN202410118463A CN117892896B CN 117892896 B CN117892896 B CN 117892896B CN 202410118463 A CN202410118463 A CN 202410118463A CN 117892896 B CN117892896 B CN 117892896B
- Authority
- CN
- China
- Prior art keywords
- path
- path planning
- node
- search
- planning period
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 108
- 238000000034 method Methods 0.000 title claims abstract description 60
- 230000002457 bidirectional effect Effects 0.000 title claims abstract description 24
- 238000009499 grossing Methods 0.000 claims abstract description 40
- 238000013138 pruning Methods 0.000 claims abstract description 21
- 238000004364 calculation method Methods 0.000 claims description 17
- 238000011156 evaluation Methods 0.000 claims description 12
- 238000012544 monitoring process Methods 0.000 claims description 10
- 230000008859 change Effects 0.000 claims description 9
- 238000001514 detection method Methods 0.000 claims description 8
- 230000001960 triggered effect Effects 0.000 claims description 4
- 238000006467 substitution reaction Methods 0.000 claims description 3
- 238000005070 sampling Methods 0.000 description 27
- 230000006870 function Effects 0.000 description 25
- 238000005457 optimization Methods 0.000 description 15
- 230000008569 process Effects 0.000 description 14
- 230000008901 benefit Effects 0.000 description 13
- 230000007246 mechanism Effects 0.000 description 12
- 238000010586 diagram Methods 0.000 description 9
- 238000004088 simulation Methods 0.000 description 6
- 230000008713 feedback mechanism Effects 0.000 description 5
- 230000007547 defect Effects 0.000 description 4
- 238000009795 derivation Methods 0.000 description 3
- 230000003044 adaptive effect Effects 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 230000004927 fusion Effects 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 230000008447 perception Effects 0.000 description 2
- 238000010845 search algorithm Methods 0.000 description 2
- 230000009471 action Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000004888 barrier function Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000010485 coping Effects 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000010606 normalization Methods 0.000 description 1
- 238000012805 post-processing Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 239000000126 substance Substances 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06Q—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES; SYSTEMS OR METHODS SPECIALLY ADAPTED FOR ADMINISTRATIVE, COMMERCIAL, FINANCIAL, MANAGERIAL OR SUPERVISORY PURPOSES, NOT OTHERWISE PROVIDED FOR
- G06Q10/00—Administration; Management
- G06Q10/04—Forecasting or optimisation specially adapted for administrative or management purposes, e.g. linear programming or "cutting stock problem"
- G06Q10/047—Optimisation of routes or paths, e.g. travelling salesman problem
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06N—COMPUTING ARRANGEMENTS BASED ON SPECIFIC COMPUTATIONAL MODELS
- G06N5/00—Computing arrangements using knowledge-based models
- G06N5/01—Dynamic search techniques; Heuristics; Dynamic trees; Branch-and-bound
Landscapes
- Engineering & Computer Science (AREA)
- Business, Economics & Management (AREA)
- Human Resources & Organizations (AREA)
- Theoretical Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Strategic Management (AREA)
- Economics (AREA)
- Physics & Mathematics (AREA)
- Computing Systems (AREA)
- Artificial Intelligence (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Evolutionary Computation (AREA)
- Data Mining & Analysis (AREA)
- Development Economics (AREA)
- Computational Linguistics (AREA)
- Game Theory and Decision Science (AREA)
- Mathematical Physics (AREA)
- Entrepreneurship & Innovation (AREA)
- Marketing (AREA)
- Operations Research (AREA)
- Quality & Reliability (AREA)
- Tourism & Hospitality (AREA)
- General Business, Economics & Management (AREA)
- Management, Administration, Business Operations System, And Electronic Commerce (AREA)
Abstract
The invention relates to the technical field of path planning, and particularly discloses a dynamic path planning method based on an improved bidirectional RRT algorithm, which comprises the following steps: determining a new node through heuristic search or random search in the current path planning period; searching all father nodes of the new node in the current path planning period by backtracking to generate a plurality of candidate paths in the current path planning period, and determining an optimal path; then sequentially determining the optimal paths in all subsequent path planning periods until the distance between the new node and the target point in the subsequent path planning period is smaller than a distance threshold; generating a complete path from a starting point to an end point according to the optimal path in all path planning periods; and smoothing and pruning the complete path from the starting point to the end point to obtain a final path. The invention can show higher planning speed, fewer expansion nodes, shorter effective path and higher smoothness in a plurality of scenes.
Description
Technical Field
The invention relates to the technical field of path planning, in particular to a dynamic path planning method based on an improved bidirectional RRT algorithm.
Background
Rapidly-exploring Random Trees (RRT) algorithm was proposed to address the search problem in complex, high-dimensional space in conventional path planning. The advantages of rapid convergence and universal applicability of the RRT algorithm have quite wide application scenes, however, the traditional RRT algorithm has some defects in practical application, wherein the most remarkable defects include:
(1) Strong randomness: the traditional RRT algorithm is too dependent on random sampling in tree expansion, so that the generated path has larger randomness, wastes calculation force and possibly does not meet the expectations in the actual application scene;
(2) The path quality is lower: because the node backtracking is not performed, the optimal path is found out through the cost function, the path searched by the traditional RRT algorithm can be excessively tortuous, and the path is overlong or is close to a non-optimal or non-optimal path such as an obstacle;
(3) Redundant nodes are more and the track is zigzag: due to the randomness of the search strategy, the tree generated by the traditional RRT algorithm contains a large number of redundant nodes, so that a great amount of calculation force is wasted in path planning; the generated path is too tortuous because the smooth processing of the track is not considered, which is not beneficial to the movement tracking of the robot in certain application scenes;
(4) Failure to effectively cope with dynamic environments and more complex map environments: conventional RRT algorithms present a significant challenge when faced with dynamic environments because their planning is based on a priori information of static maps. When the environment changes, such as moving obstacles or dynamic obstacles, the conventional RRT algorithm cannot adjust the planning strategy in real time, resulting in failure of the generated path. Conventional RRT algorithms have limitations in relatively complex large map environments, particularly in scenarios requiring long distance planning or involving multiple obstacles. Because of the randomness of the algorithm, a great deal of time is needed to search for an effective path at this time, and the quality and planning speed of the path are affected by the map size and the difficult-to-predict obstacle distribution;
In existing approaches, some researchers have proposed methods to improve RRT algorithms. For example Jayasree et al store nodes and edges by building a tree structure to improve search efficiency and use spline interpolation for path smoothing. The Pei et al adopts an artificial potential field method to optimize the sampling process, reduce the calculated amount, execute target deviation sampling under the action of potential field force, and adjust the sampling area. The method aims at solving the problems of randomness, path tortuosity, redundant nodes and the like of the traditional RRT algorithm, so as to improve the efficiency and quality of path planning, and adapt to different application scenes and challenges.
The disadvantages of the prior art are mainly manifested in the following aspects:
(1) Local minimum problem: the existing technical scheme is easily affected by the local minimum problem, namely, in some cases, the robot may be trapped in the potential field with the local minimum, and the robot is difficult to jump out of the local minimum. This may result in insufficient randomness of RRT such that the quality of the path planned by the robot is limited;
(2) Path feasibility uncertainty: the existing technical scheme has higher requirements on the perception and environment modeling of the robot, and perception errors and modeling uncertainties can exist. At the same time, this uncertainty is transferred to the path planning, resulting in the generated path not being feasible in actual execution or requiring additional corrections;
(3) The computational complexity is high: the existing technical scheme has higher calculation complexity, and particularly in a large-scale and complex environment, a large amount of calculation resources can be consumed. This will lead to a decrease in the efficiency of the overall algorithm, especially in scenarios requiring real-time planning;
(4) Too relying on artificial potential fields: the artificial potential field method is mainly based on modeling obstacles in the environment, guiding path planning by simulating interactions between objects. However, too much reliance on artificial potential fields will result in over assumptions about the environment, especially in complex or dynamic environments, the potential field model may be inaccurate, resulting in non-ideal planning results;
(5) Dynamic feedback is still not considered: most of the prior art schemes optimize sampling strategies, path smoothing and track selection, but the dynamic planning strategies are not considered to be adjusted according to environment dynamics so as to actively adapt to dynamic planning of various scenes.
Disclosure of Invention
The invention aims to overcome the defects in the prior art, and provides a dynamic path planning method based on an improved bidirectional RRT algorithm, so as to solve the technical problems of low efficiency, path tortuosity and more redundant nodes in path planning of the traditional rapid search random tree (RRT) algorithm.
As a first aspect of the present invention, there is provided a dynamic path planning method based on an improved bidirectional RRT algorithm, the dynamic path planning method based on an improved bidirectional RRT algorithm including:
step S1: setting a starting point, an ending point, a searching step length, a probability threshold value and a distance threshold value of path planning;
step S2: selecting heuristic search or random search according to the probability threshold value, and determining a new node through heuristic search or random search in the current path planning period;
Step S3: searching all father nodes of a new node in a current path planning period by backtracking to generate a plurality of candidate paths in the current path planning period, and determining an optimal path from the plurality of candidate paths in the current path planning period; and then circularly executing the steps S2-S3, and sequentially determining the optimal paths in all subsequent path planning periods until the distance between the new node and the target point in the subsequent path planning period is smaller than the distance threshold;
step S4: generating a complete path from a starting point to an end point according to the optimal path in all path planning periods;
Step S5: and smoothing and pruning the complete path from the starting point to the end point to obtain a final path from the starting point to the end point.
Further, the selecting heuristic search or random search according to the probability threshold value, determining a new node through heuristic search or random search in the current path planning period, and further includes:
Calculating Euclidean distance h dist between the new node and the target point in the previous path planning period, and calculating distance h obs between the new node and the obstacle in the current direction in the previous path planning period;
Calculating a distance index H dist and an obstacle index H obs in the current path planning period; wherein, the calculation formula is as follows:
Wherein d max is the distance between the start point and the end point, d min is the distance threshold, and k is the adjustment parameter;
The overall heuristic search probability P (H) is calculated, and the calculation formula is as follows:
P(H)＝α*Hdist+β*Hobs
Wherein alpha and beta are respectively the weighting coefficients of the evaluation;
Selecting heuristic search or random search according to the overall heuristic search probability P (H) and the probability threshold P tar;
When the heuristic search probability P (H) is larger than the probability threshold P tar, determining a new node through heuristic search x goal in the current path planning period;
When the heuristic search probability P (H) is larger than the random probability value P and smaller than the probability threshold value P tar, determining a new node through heuristic search x goal in the current path planning period;
When the heuristic search probability P (H) is smaller than the random probability value P, a new node is determined by random search SAMPLEFREE () in the current path planning period.
Further, the determining a new node through heuristic search in the current path planning period further includes:
Calculating an angle theta 1 of a new node of the current tree and an angle theta 2 of a new node of another tree;
And determining a new node of the next path planning period according to the searching step length in the path direction along the angle theta 1 or the angle theta 2.
Further, the step of finding all parent nodes of the new node in the current path planning period by backtracking to generate a plurality of candidate paths in the current path planning period, and determining an optimal path from the plurality of candidate paths in the current path planning period, further includes:
searching all father nodes in a certain radius range with the new node as a center when the new node in the current path planning period explores the search space so as to generate a plurality of candidate paths in the current path planning period;
the path length cost value length, the obstacle cost value obstacle and the smooth cost value smooth of each candidate path in the current path planning period are calculated according to the following calculation formulas:
length＝∑distance(node_i,node_i+1)
obstacle＝∑distance(node_i,obstacle)
smooth＝∑angle(|node_i,node_i+1|)
wherein distance (node_i, node_i+1) refers to the distance between the i-th node and the i+1-th node on each candidate path, distance (node_i, obstacle) refers to the distance between the i-th node and the obstacle on each candidate path, angle (i node_i, node_i+1) refers to the absolute value of the difference between the angle of the i-th node and the angle of the i+1-th node on each candidate path;
According to the path length cost value length, the obstacle cost value obstacle and the smooth cost value smooth of each candidate path in the current path planning period, the total cost value C of each candidate path in the current path planning period is calculated, and the calculation formula is as follows:
C＝w1*length+w2*obstacle+w3*smooth
Wherein w 1、w2、w3 is a weight coefficient, respectively;
And determining a candidate path with the minimum total cost value C from the plurality of candidate paths in the current path planning period according to the total cost value C of each candidate path in the current path planning period, and taking the candidate path with the minimum total cost value C as the optimal path in the current path planning period.
Further, the method further comprises the following steps:
1) Recording a total cost value C of an initial path planning period: recording the total cost value C in the initial five path planning periods, and calculating the average value as a reference value;
2) Monitoring the subsequent change of the total cost value C: continuously monitoring a total cost value C in each subsequent path planning period;
3) Triggering an automatic adjustment search strategy: comparing the total cost value C in the current path planning period with a reference value, triggering an automatic adjustment search strategy to reduce the search step length, and determining a new node through random search in the four subsequent path planning periods if the total cost value C in the current path planning period is larger than the reference value or the total cost value C in the four subsequent path planning periods continuously rises; if the total cost value C in the following four path planning periods still continuously rises or exceeds an emergency threshold C emergency after the automatic adjustment strategy is triggered, searching is directly carried out towards the offset reverse direction of the target point, and the searching step length is greatly increased;
4) Dynamically updating a reference value: taking the average value of the total cost value C of the latest five path planning periods as a reference value;
5) Continuing monitoring and feedback: after the search strategy is automatically adjusted, the change of the subsequent total cost value C is continuously monitored so as to ensure that the quality of path planning is continuously improved.
Further, the smoothing and pruning process is performed on the complete path from the starting point to the end point to obtain a final path from the starting point to the end point, and the method further includes:
smoothing the complete path from the starting point to the end point through a path smoothing algorithm to obtain a smooth path from the starting point to the end point;
Judging whether the nodes on the smooth path from the starting point to the end point are redundant points or not through a collision detection algorithm, and deleting the redundant points based on the principle of straight line path approximation so as to obtain a final path from the starting point to the end point.
Further, the smoothing algorithm smoothes the complete path from the start point to the end point to obtain a smoothed path from the start point to the end point, and further includes:
1) Selecting data points: taking N nodes on the complete path from the starting point to the end point as N data points to form a time sequence { x (1), x (2),. The number of the nodes, x (N) }, taking the data points in a window, namely { x (i-k), x (i-k+1),. The number of the nodes, x (i+k) }, wherein k is half of the size of the window, and rounding down, with each data point x (i) in the time sequence as a center;
2) Calculating the median: calculating the median of the data points in the window, namely, after the data points in the window are arranged according to the size, taking the value positioned in the middle position;
3) The median substitution is adopted: replacing the original data point x (i) with the calculated median value to obtain a smoothed data point;
4) Repeating the steps: with the movement of the dynamic window, a median smoothing operation is performed on all data points in the time series.
Further, the determining, by the collision detection algorithm, whether the node on the smooth path from the start point to the end point is a redundant point further includes:
For each pair of non-adjacent nodes on the smooth path, if the path between them is not affected by an obstacle, the intermediate node of each pair of non-adjacent nodes is marked as a redundant point.
The dynamic path planning method based on the improved bidirectional RRT algorithm has the following advantages:
(1) Two-way heuristic search is adopted: and introducing a bidirectional heuristic search strategy, judging a sampling strategy by a target bias method and using a specific heuristic function and a probability threshold value, and guiding two random trees to alternately expand between a starting point and an ending point. This helps to reduce randomness and improve controllability and accuracy of path planning. The problem of strong randomness of the traditional RRT algorithm is solved;
(2) Backtracking nodes judge paths: and searching the backtracking path by taking the generated new node as the circle center, setting a cost function to compare the searched multiple alternative paths, and selecting the path with the lowest cost. The problem that the quality of the traditional RRT algorithm along with the path is lower is solved;
(3) Dynamic feedback optimization: and introducing a dynamic feedback optimization mechanism, and adjusting the search rule in real time to adapt to a dynamic environment. When the target bias sampling fails for a plurality of times, triggering dynamic feedback optimization, actively changing the sampling strategy, and rapidly walking out of the obstacle trap in the narrow space. The problem that the traditional RRT algorithm cannot effectively cope with a dynamic environment and a relatively complex large map environment is solved;
(4) Path smoothing and pruning: and performing redundant point pruning on the generated path by adopting a linear approximation method, and removing unnecessary nodes. And optimizing the planned path by adopting a moving median smoothing algorithm to ensure that the final path is smoother and more efficient. The problems of more redundant points and track smoothing post-processing of the traditional RRT algorithm are solved.
Drawings
The accompanying drawings are included to provide a further understanding of the invention, and are incorporated in and constitute a part of this specification, illustrate the invention and together with the description serve to explain, without limitation, the invention.
Fig. 1 is a flowchart of a dynamic path planning method based on an improved bidirectional RRT algorithm provided by the present invention.
Fig. 2 is a flowchart of a specific implementation manner of the dynamic path planning method based on the improved bidirectional RRT algorithm provided by the present invention.
Fig. 3 is a schematic diagram of a bidirectional RRT algorithm extended node process.
FIG. 4 is a schematic diagram of an angle-based target bias search.
FIG. 5 is a schematic diagram of a novel node backtracking search to generate a preselected route.
Fig. 6 is a schematic diagram of a redundant point pruning process.
Fig. 7 is a schematic view of a basic obstacle avoidance scenario planning space.
Fig. 8 is a schematic diagram of three algorithm path plans in a single-channel obstacle environment.
Fig. 9 is a schematic diagram of three algorithm path plans in a complex multi-channel obstacle environment.
Fig. 10 is a schematic view of an obstacle trap scene planning space.
Fig. 11 is a schematic diagram of three algorithm path plans in an obstacle trap environment.
Detailed Description
It should be noted that, without conflict, the embodiments of the present invention and features of the embodiments may be combined with each other. The invention will be described in detail below with reference to the drawings in connection with embodiments.
In order that those skilled in the art will better understand the present invention, a technical solution in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in which it is apparent that the described embodiments are only some embodiments of the present invention, not all embodiments. All other embodiments, which can be made by those skilled in the art based on the embodiments of the present invention without making any inventive effort, shall fall within the scope of the present invention.
It should be noted that the terms "first," "second," and the like in the description and the claims of the present invention and the above figures are used for distinguishing between similar objects and not necessarily for describing a particular sequential or chronological order. It is to be understood that the data so used may be interchanged where appropriate in order to describe the embodiments of the invention herein. Furthermore, the terms "comprises," "comprising," and "having," and any variations thereof, are intended to cover a non-exclusive inclusion, such that a process, method, system, article, or apparatus that comprises a list of steps or elements is not necessarily limited to those steps or elements expressly listed but may include other steps or elements not expressly listed or inherent to such process, method, article, or apparatus.
In this embodiment, a dynamic path planning method based on an improved bidirectional RRT algorithm, DFCO-RRT-Connect is provided, and fig. 1 is a flowchart of a dynamic path planning method based on an improved bidirectional RRT algorithm, DFCO-RRT-Connect, provided by the invention. As shown in fig. 1, the dynamic path planning method based on the improved bidirectional RRT algorithm, DFCO-RRT-Connect, includes:
step S1: setting a starting point, an ending point, a searching step length, a probability threshold value and a distance threshold value of path planning;
step S2: selecting heuristic search or random search according to the probability threshold value, and determining a new node through heuristic search or random search in the current path planning period;
Preferably, as shown in fig. 2, the selecting a heuristic search or a random search according to the probability threshold value determines a new node through the heuristic search or the random search in the current path planning period, and further includes:
in the path planning problem, the invention designs a heuristic function by taking two key distance indexes of Euclidean distance of a target point and distance of an obstacle in the current direction into consideration, so as to guide a search algorithm to make decisions on directivity and obstacle avoidance when searching a state space: specifically, a Euclidean distance h dist between the new node and the target point in the previous path planning period is calculated, and a distance h obs between the new node and the obstacle in the current direction in the previous path planning period is calculated;
hdist＝dx2+dy2
The normalization operation is adopted for the heuristic function, the further the heuristic value is required to be from the target point, the further the heuristic value is from the obstacle in the direction, and the smaller the heuristic value is: specifically, a distance index H dist and an obstacle index H obs in the current path planning period are calculated; wherein, the calculation formula is as follows:
Wherein d max is the distance between the starting point and the end point, d min is the distance threshold, k is the adjustment parameter, and the slope of the obstacle index H obs is controlled to adjust the change speed of the heuristic value;
The overall heuristic search probability P (H) is calculated, and the calculation formula is as follows:
P(H)＝α*Hdist+β*Hobs
Wherein alpha and beta are respectively the weighting coefficients of the evaluation;
Selecting heuristic search or random search according to the overall heuristic search probability P (H) and the probability threshold P tar; the search space can be explored more comprehensively and efficiently with hybrid searches combining heuristic and random searches:
Wherein, a probability threshold value P tar =0.6 is set, a random probability value P is obtained according to uniform random distribution, and the value range of the random probability value P is 0-1;
When the heuristic search probability P (H) is larger than the probability threshold P tar, determining a new node through heuristic search x goal in the current path planning period;
When the heuristic search probability P (H) is larger than the random probability value P and smaller than the probability threshold value P tar, determining a new node through heuristic search x goal in the current path planning period;
When the heuristic search probability P (H) is smaller than the random probability value P, a new node is determined by random search SAMPLEFREE () in the current path planning period.
It should be noted that, as shown in fig. 3, the bidirectional RRT algorithm is a path planning algorithm, and aims to search from the start point and the end point simultaneously, so as to accelerate the path planning process.
Specifically, the determining a new node through heuristic search in the current path planning period further includes:
Calculating an angle theta 1 of a new node of the current tree and an angle theta 2 of a new node of another tree; the heuristic search method is angle-based target bias search, and in order to solve the problem of randomness of the RRT-Connect algorithm in sampling, angle-based target bias sampling is added to guide the growth of a random tree. As shown in fig. 4, the angles θ 1 and θ 2 are calculated using an arctangent function by calculating the x, y axis coordinate difference of the current tree's latest node and another tree's latest node. The update tree is switched in the next iteration cycle, and angles theta 1 and theta 2 are calculated using the same method:
And determining a new node of the next path planning period according to the searching step length in the path direction along the angle theta 1 or the angle theta 2.
Step S3: searching all father nodes of a new node in a current path planning period by backtracking to generate a plurality of candidate paths in the current path planning period, and determining an optimal path from the plurality of candidate paths in the current path planning period; and then circularly executing the steps S2-S3, and sequentially determining the optimal paths in all subsequent path planning periods until the distance between the new node and the target point in the subsequent path planning period is smaller than the distance threshold;
Preferably, as shown in fig. 2, the step of finding all parent nodes of the new nodes in the current path planning period by backtracking to generate a plurality of candidate paths in the current path planning period, and determining the best path from the plurality of candidate paths in the current path planning period further includes:
And (3) generating a plurality of paths from the newly generated nodes to the parent nodes by utilizing search backtracking, and calculating and selecting the paths by utilizing a cost function so as to improve the path quality of the search tree. The specific process is as follows:
(1) Searching all father nodes in a certain radius range with the new node as a center when the new node in the current path planning period explores the search space, so as to generate a plurality of candidate paths in the current path planning period, and optimizing the subsequent path selection process; once a new sample point is generated in the search space, a search area or radius is typically defined that covers a range around the new sample point, as shown in fig. 5. By searching for a parent sampling point within the region, a plurality of preselected paths are generated that represent possible connections from the parent sampling point to the new sampling point.
(2) The pre-selected paths generated may include a plurality of paths representing different connection options. In order to select the best path, collision detection is first performed on the path. And secondly, setting an evaluation function considering the path length cost, the obstacle cost and the smooth cost for evaluating the quality of each path. Finally, according to the value of the cost function, the path with the lowest cost is selected as the final path selection. Among the several trajectories obtained above, by setting the path length cost, the obstacle cost, and the smoothing cost: the path length cost value length, the obstacle cost value obstacle and the smooth cost value smooth of each candidate path in the current path planning period are calculated according to the following calculation formulas:
length＝∑distance(node_i,node_i+1)
obstacle＝∑distance(node_i,obstacle)
smooth＝∑angle(|node_i,node_i+1|)
wherein distance (node_i, node_i+1) refers to the distance between the i-th node and the i+1-th node on each candidate path, distance (node_i, obstacle) refers to the distance between the i-th node and the obstacle on each candidate path, angle (i node_i, node_i+1) refers to the absolute value of the difference between the angle of the i-th node and the angle of the i+1-th node on each candidate path; the path length cost value length is the total path length in the search area, and the obstacle cost value obstacle is the sum of the distances from the connecting path nodes to the obstacles in the search area; the smoothing cost value smooths is the sum of absolute values of angle differences of the connection path nodes within the search area.
According to the path length cost value length, the obstacle cost value obstacle and the smooth cost value smooth of each candidate path in the current path planning period, the total cost value C of each candidate path in the current path planning period is calculated, and the calculation formula is as follows:
C＝w1*length+w2*obstacle+w3*smooth
Wherein w 1、w2、w3 is a weight coefficient, respectively;
And determining a candidate path with the minimum total cost value C from the plurality of candidate paths in the current path planning period according to the total cost value C of each candidate path in the current path planning period, and taking the candidate path with the minimum total cost value C as the optimal path in the current path planning period.
Specifically, as shown in fig. 2, the method further includes:
The goal of the dynamic feedback mechanism is to adjust the parameters based on feedback information of the actual path planning performance. When a narrow road section occurs or a search faces a local minimum problem, the path algorithm is prone to trap into an obstacle trap. The dynamic feedback mechanism is introduced to improve the adaptability and efficiency of the algorithm, and the specific process is as follows:
1) Recording a total cost value C of an initial path planning period: recording the total cost value C in the initial five path planning periods, and calculating the average value as a reference value;
2) Monitoring the subsequent change of the total cost value C: continuously monitoring a total cost value C in each subsequent path planning period;
3) Triggering an automatic adjustment search strategy: comparing the total cost value C in the current path planning period with a reference value, triggering an automatic adjustment search strategy to reduce the search step epsilon, and determining a new node through random search in the four subsequent path planning periods if the total cost value C in the current path planning period is larger than the reference value or the total cost value C in the four subsequent path planning periods continuously rises; if the total cost value C in the following four path planning periods still continuously rises or exceeds an emergency threshold C emergency after the automatic adjustment strategy is triggered, searching is directly carried out towards the offset reverse direction of the target point, and the searching step epsilon is greatly increased;
4) Dynamically updating a reference value: taking the average value of the total cost value C of the latest five path planning periods as a reference value;
5) Continuing monitoring and feedback: after the search strategy is automatically adjusted, the change of the subsequent total cost value C is continuously monitored so as to ensure that the quality of path planning is continuously improved.
Step S4: generating a complete path from a starting point to an end point according to the optimal path in all path planning periods;
Step S5: and smoothing and pruning the complete path from the starting point to the end point to obtain a final path from the starting point to the end point.
Preferably, the smoothing and pruning process is performed on the complete path from the starting point to the end point to obtain a final path from the starting point to the end point, and the method further includes:
Under the condition that a large number of obstacles exist or a planned path is longer, an initial path planned by the DFCO-RRT-Connect algorithm is still tortuous, and due to randomness of the algorithm, a plurality of redundant points exist in the generated initial path. The core idea of the median smoothing method is to replace each data point in the original data with the median of the data within the dynamic window. The specific process is as follows:
smoothing the complete path from the starting point to the end point through a path smoothing algorithm to obtain a smooth path from the starting point to the end point;
Specifically, the smoothing algorithm smoothes the complete path from the start point to the end point to obtain a smoothed path from the start point to the end point, and further includes:
1) Selecting data points: taking N nodes on the complete path from the starting point to the end point as N data points to form a time sequence { x (1), x (2),. The number of the nodes, x (N) }, taking the data points in a window, namely { x (i-k), x (i-k+1),. The number of the nodes, x (i+k) }, wherein k is half of the size of the window, and rounding down, with each data point x (i) in the time sequence as a center;
2) Calculating the median: calculating the median of the data points in the window, namely, after the data points in the window are arranged according to the size, taking the value positioned in the middle position;
3) The median substitution is adopted: replacing the original data point x (i) with the calculated median value to obtain a smoothed data point;
4) Repeating the steps: with the movement of the dynamic window, a median smoothing operation is performed on all data points in the time series.
Judging whether the nodes on the smooth path from the starting point to the end point are redundant points or not through a collision detection algorithm, and deleting the redundant points based on the principle of straight line path approximation so as to obtain a final path from the starting point to the end point.
Specifically, the determining, by the collision detection algorithm, whether the node on the smooth path from the start point to the end point is a redundant point further includes:
For each pair of non-adjacent nodes on the smooth path, if the path between them is not affected by an obstacle, the intermediate node of each pair of non-adjacent nodes is marked as a redundant point.
When the path is generated, if the connection line in the two non-adjacent nodes is not affected by the obstacle, the path point is regarded as a redundant point. As shown in fig. 6, X 2 and X 3 are redundant points in the figure. Because for each pair of non-adjacent nodes, the path between them can be approximately equivalent to a straight line if not affected by an obstacle, the intermediate nodes do not have a significant impact on the path effectiveness, and so these intermediate nodes are labeled as redundant nodes.
The invention performs algorithm simulation in MATLAB R2020 a. The computer configuration used for the experiment is as follows: the processor is AMD Ryzen 74800H, the main frequency is 2.90GHz, the memory is 16GB, and the operating system is 64-bit Windows 10.
In order to verify the effectiveness of the proposed DFCO-RRT-Connect algorithm in practical applications, the invention detects RRT, H-RRT and the algorithm from basic performance, capability of solving local minimum problem and capability of labyrinth routing respectively. Due to the randomness of sampling, the original algorithm and the improved algorithm are applied to 20 simulation experiments under different scenes respectively, and the performance indexes of the simulation experiments are analyzed.
(1) Basic performance evaluation: and setting single-channel and complex multi-channel obstacle avoidance scenes. The map size is 800×800 (dimensionless), and as shown in fig. 7, the black portion is an obstacle. The starting point and target point coordinates are (100 ) and (750 ), respectively, the expansion step size epsilon is 20, and the distance threshold delta is 20. Fig. 8 and 9 show schematic diagrams of path planning results obtained by using basic RRT, H-RRT and the DFCO-RRT-Connect algorithm presented herein in single-channel and complex multi-channel obstacle scenarios, respectively. And under a single-channel obstacle avoidance scene, carrying out experimental simulation for 20 times. The three algorithm performance experimental data are shown in table 1. And under a complex multichannel obstacle avoidance scene, carrying out experimental simulation for 20 times. The three algorithm performance experimental data are shown in table 2.
Table 1 (Experimental data of planning algorithm performance in single channel obstacle avoidance environment)
Table 2 (planning algorithm performance experimental data under complex multichannel obstacle avoidance environment)
(2) To test the performance of the algorithm dynamic feedback optimization of the present invention, an obstacle trap scenario was set, with a map size of 630 x 1050 (dimensionless), as shown in fig. 10. The starting point and target point coordinates are (70, 550) and (10, 550), respectively, the expansion step size epsilon is 20, and the distance threshold delta is 20. In the obstacle trap scene, a schematic diagram of a path planning result obtained by adopting a basic RRT algorithm, an H-RRT algorithm and the algorithm of the invention is shown in fig. 11. And under the scene of the obstacle trap, carrying out experimental simulation for 20 times. The three algorithm performance experimental data are shown in table 3.
Table 3 (Experimental data for planning algorithm performance in obstacle trap environment)
In summary, compared with the RRT algorithm and the H-RRT algorithm, the algorithm provided by the invention obviously reduces the node number and the search time. Particularly in the complex and long scene with obstacle traps, the algorithm has obvious advantages due to a dynamic feedback optimization mechanism: compared with the H-RRT algorithm, the algorithm of the invention reduces 48.80% of expansion nodes and 80.93% of search time under the complex multi-channel obstacle avoidance scene respectively: the expansion node of 92.15% and the search time of 97.76% are reduced in the case of long and narrow existence of obstacle trap.
It should be noted that, the dynamic feedback mechanism in the present invention may also consider using other optimization methods, such as a PID controller or Model Predictive Control (MPC), etc.
It should be noted that the present invention may also use spline interpolation, bezier curve, etc. for smoothing the path.
It should be noted that the RRT (Rapidly-exploring Random Trees) algorithm is a random search algorithm for path planning. The core idea is to quickly explore the potential path space through random sampling and continuous expansion of the tree. The algorithm starts from a starting point, randomly generates sampling points, and then extends to the sampling points along the path by finding nearest neighbor nodes, ensuring that the path does not intersect with obstacles. The generated new node is added to the tree and the above steps are repeated until a predetermined condition is met. Finally, the planned path is found by backtracking from the end point to the start point. The randomness of the RRT algorithm makes it suitable for path planning problems in high-and complex environments, but may also lead to the generated paths not being smooth enough and having some uncertainty.
It should be noted that heuristic search is a search strategy based on experience and heuristic information, and aims to effectively find a solution to the problem. In heuristic searches, a heuristic function is used to estimate the "goodness" of each search state to guide the search toward a direction that is more likely to produce a good solution. This search method is generally applied to complex problems, where a method of comprehensively searching all possibilities may be inefficient, and heuristic search is a strategy of using a priori knowledge in the search process, guiding the search by estimating the quality of the state to find a solution more efficiently.
It should be noted that dynamic planning is an algorithm method for path planning, which divides a problem into small parts, gradually calculates and stores an optimal solution, and finally obtains an optimal path of the whole problem. In path planning, this includes defining the states of the locations and movements, establishing the cost or optimal value of the movements between the locations, and storing the results of the solved problem. By recursive calculation, the optimal path can be efficiently found by dynamic planning, and the optimal path can be used for solving the problems of the shortest path or the optimal path.
The invention adopts a bidirectional heuristic RRT searching method based on the traditional RRT algorithm to reduce the randomness of expansion based on the target bias strategy of the current node and the target point angle. And adding a path length constraint, an obstacle distance constraint and a cost function of smoothing constraint to calculate a plurality of paths generated by the new node through backtracking the father node, and finding out the optimal path. If the target bias sampling fails for a plurality of times in a narrow space or in a local minimum problem, a dynamic feedback optimization mechanism is triggered. The sampling strategy is actively changed to rapidly walk out of the obstacle trap in the narrow space. And finally, smoothing and pruning the planned path to improve the path quality.
In an embodiment of the invention, the advantage derivation of intelligent sampling extension: introducing a target bias strategy: by calculating angles among tree nodes and adopting an arctangent function, a target bias strategy is introduced, so that randomness is effectively reduced, and the selection of sampling points is more directional; reasonable application of probability conditions: by combining heuristic search and random search, a path search strategy is selected based on probability conditions, so that the system can flexibly adjust the search strategy under different conditions, and the flexibility and global planning of search are improved.
In the embodiment of the invention, the advantage deduction of the backtracking and evaluation path: flexibility of alternate track generation: by searching for a parent sample point in the vicinity of the new sample point, the algorithm is able to construct multiple alternate tracks, which are possible connections from the parent sample point to the new sample point. The adoption of the mechanism is more flexible, the search space can be fully utilized, and the diversity and coverage of alternative paths are improved; advantages of path evaluation: in path planning, the design of the cost function takes into account a number of key factors, including path length, obstacle distance, and smoothness. By evaluating the cost function of the pre-selected paths generated, the algorithm is able to select the path with the lowest cost as the final path selection. This path evaluation mechanism improves the path quality of the search tree, making the final selected path more optimal and efficient.
In the embodiment of the invention, the advantage derivation of dynamic feedback optimization: real-time performance feedback: and setting a cost function value record and a threshold value of an initial period, and realizing monitoring of real-time performance feedback. The system can timely detect the change of path planning performance, and dynamically adjust search parameters; adaptive search strategy: by judging the rise of the cost function value and triggering the automatic adjustment search strategy, the system can quickly make adjustment when facing a narrow road section or a local minimum problem, and the effectiveness and adaptability of path planning are ensured.
In the embodiment of the invention, the advantages of path smoothing and pruning are deduced: the superiority of the moving median algorithm: introducing a moving median algorithm to carry out smoothing treatment on the path, and effectively eliminating the jitter of the path by replacing each data point in the original data with the median of the data in the dynamic window, so that the generated path is smoother; rationality of redundant node pruning: intermediate nodes with little influence on the path effectiveness are deleted by a redundant node pruning method with approximate straight lines, and redundant information on the path is reduced, so that the readability and the navigation efficiency of the path are further improved.
In the embodiment of the invention, the derivation of the comprehensive advantages: comprehensive advantages of multi-mechanism fusion: the invention effectively makes up the defects of the traditional RRT algorithm and the existing technical scheme when facing complex environments and dynamic obstacles through smart fusion of a plurality of mechanisms such as intelligent sampling expansion, backtracking and path evaluation, dynamic feedback optimization, path smoothing and pruning in intelligent path planning, and ensures that the system is excellent in various scenes.
Therefore, from the deduction analysis of aspects such as intelligent sampling expansion, backtracking and path evaluation, dynamic feedback optimization, path smoothing and pruning, the invention has remarkable advantages in the field of intelligent path planning, not only improves the searching efficiency, but also optimizes the quality of generated paths, and brings innovation and practicability to practical application in the fields such as robot navigation, autonomous unmanned vehicles and the like. Through the advantages, the DFCO-RRT-Connect algorithm is obviously superior in terms of improving search efficiency, path quality and adaptability, and a more efficient and comprehensive solution is provided for the path planning problem.
In the embodiment of the invention, a bidirectional heuristic search based on dynamic feedback optimization improves the RRT algorithm (DFCO-RRT-Connect) to cope with challenges in path planning in two-dimensional space. Technically, we first strive to guide two random trees to alternately spread between a start point and an end point with a specific probability through a bi-directional heuristic search strategy, calculate a plurality of possible paths through a backtracking mechanism and combining a cost function, and select an optimal path from the paths. Secondly, we introduce a dynamic feedback optimization mechanism to adapt to dynamic environments and avoid trapping in obstacle traps. The mechanism actively changes the sampling strategy by adjusting the searching rule, and improves the coping capability of the algorithm to complex scenes. Finally, we prune the redundant points of the generated path and employ a moving median smoothing algorithm to further optimize the quality and smoothness of the path. Comprehensively, the patent aims to comprehensively solve the technical problems in the traditional RRT algorithm through the DFCO-RRT-Connect algorithm and realize efficient, accurate and smooth path planning in various scenes.
The invention provides a dynamic path planning method based on an improved bidirectional RRT algorithm (Rapidly-exploring Random Trees Connect, RRT-Connect). Firstly, bidirectional heuristic search is adopted, and two random trees are guided to be alternately expanded through a target bias strategy. And then, backtracking the parent node through the generated new node to generate a plurality of candidate tracks, and selecting an optimal path by using a cost function. And secondly, introducing a dynamic feedback optimization mechanism in the planning process, actively adjusting the planning strategy by utilizing the change of the cost function so as to adapt to the dynamic environment, prevent the obstacle trap from being trapped, and comprehensively improve the path planning performance. Finally, the planned path is further optimized by path smoothing and pruning. These features enable DFCO-RRT-Connect to exhibit higher planning speed, fewer expansion nodes, shorter effective paths, and higher smoothness in multiple scenarios.
The invention provides a dynamic path planning method based on an improved bidirectional RRT algorithm, namely DFCO-RRT-Connect, (1) bidirectional heuristic search: reducing the randomness of the algorithm sampling by adopting a target bias strategy, setting a heuristic function and a probability threshold value to judge probability conditions, and adopting heuristic search or random search; (2) backtracking judgment path: searching and backtracking from the newly created node circle may include a plurality of pre-selected paths that represent different connection options. In order to select the best path, collision detection is first performed on the path. And secondly, setting an evaluation function considering the path length cost, the obstacle cost and the smooth cost for evaluating the quality of each path. Finally, selecting the path with the lowest cost as the final path selection according to the value of the cost function; (3) dynamic feedback mechanism: the cost function value of the first five cycles is recorded and the average value thereof is calculated as a reference value. Comparing the subsequent cost values by the reference value, and automatically adjusting the planning rule if the cost value is continuously increased or the value is increased in the next period to exceed a threshold value; and (4) smoothing and redundant point clipping: the method adopts a movable median algorithm and a redundant node pruning method with straight line approximation, so that the paths are smoother, the effective paths are shorter, and the number of nodes is smaller. Therefore, the DFCO-RRT-Connect algorithm of the invention aims at reducing randomness and improving controllability, is adaptive to dynamic environment, and improves the regularity of dynamic barriers by adjusting search rules in real time. The strategy of path smoothing and pruning further improves the quality of the planned path making it smoother and more efficient. It is therefore an object of the present invention to overcome the potential drawbacks of the prior art improvements by an integrated innovative strategy, providing a more comprehensive, robust path planning solution.
The invention provides a dynamic path planning method based on an improved bidirectional RRT algorithm, namely DFCO-RRT-Connect, (1) an intelligent sampling expansion mechanism: the randomness of the algorithm sampling is reduced through the target bias strategy, and the intelligent searching guide is realized by combining the heuristic function and the probability threshold value, so that the efficiency and the accuracy of path searching are improved. (2) dynamic feedback optimization mechanism: and a dynamic feedback mechanism is introduced, and parameters are adjusted according to feedback information of actual path planning performance so as to solve the path planning problem in a dynamic environment and improve the adaptability and efficiency of the algorithm. (3) advantages of backtracking and evaluation paths: by adopting a mode of backtracking parent nodes to generate standby tracks and combining a cost function to evaluate and select paths, the flexibility and diversity of path selection are improved, and the global optimization level of path planning is improved. And (4) redundant node pruning and path smoothing: and the path is smoothed by using a moving median algorithm, and meanwhile, a redundant node pruning method with straight line approximation is adopted, so that the generated path is smoother, redundant points are reduced, and the feasibility and the execution performance of the path are improved.
It is to be understood that the above embodiments are merely illustrative of the application of the principles of the present invention, but not in limitation thereof. Various modifications and improvements may be made by those skilled in the art without departing from the spirit and substance of the invention, and are also considered to be within the scope of the invention.
Claims (6)
1. The dynamic path planning method based on the improved bidirectional RRT algorithm is applied to robot path planning, and is characterized by comprising the following steps of:
step S1: setting a starting point, an ending point, a searching step length, a probability threshold value and a distance threshold value of path planning;
step S2: selecting heuristic search or random search according to the probability threshold value, and determining a new node through heuristic search or random search in the current path planning period;
Step S3: searching all father nodes of a new node in a current path planning period by backtracking to generate a plurality of candidate paths in the current path planning period, and determining an optimal path from the plurality of candidate paths in the current path planning period; and then circularly executing the steps S2-S3, and sequentially determining the optimal paths in all subsequent path planning periods until the distance between the new node and the target point in the subsequent path planning period is smaller than the distance threshold;
step S4: generating a complete path from a starting point to an end point according to the optimal path in all path planning periods;
Step S5: smoothing and pruning the complete path from the starting point to the end point to obtain a final path from the starting point to the end point;
wherein, the selecting heuristic search or random search according to the probability threshold value, determining a new node through heuristic search or random search in the current path planning period, further comprises:
Calculating Euclidean distance h dist between the new node and the target point in the previous path planning period, and calculating distance h obs between the new node and the obstacle in the current direction in the previous path planning period;
Calculating a distance index H dist and an obstacle index H obs in the current path planning period; wherein, the calculation formula is as follows:
Wherein d max is the distance between the start point and the end point, d min is the distance threshold, and k is the adjustment parameter;
The overall heuristic search probability P (H) is calculated, and the calculation formula is as follows:
P(H)＝α*Hdist+β*Hobs
Wherein alpha and beta are respectively the weighting coefficients of the evaluation;
Selecting heuristic search or random search according to the overall heuristic search probability P (H) and the probability threshold P tar;
When the heuristic search probability P (H) is larger than the probability threshold P tar, determining a new node through heuristic search x goal in the current path planning period;
When the heuristic search probability P (H) is larger than the random probability value P and smaller than the probability threshold value P tar, determining a new node through heuristic search x goal in the current path planning period;
When the heuristic search probability P (H) is smaller than the random probability value P, determining a new node through random search SAMPLEFREE () in the current path planning period;
Wherein, the searching all father nodes of the new node in the current path planning period by backtracking to generate a plurality of candidate paths in the current path planning period, and determining the best path from the plurality of candidate paths in the current path planning period, further comprises:
searching all father nodes in a certain radius range with the new node as a center when the new node in the current path planning period explores the search space so as to generate a plurality of candidate paths in the current path planning period;
the path length cost value length, the obstacle cost value obstacle and the smooth cost value smooth of each candidate path in the current path planning period are calculated according to the following calculation formulas:
legnth＝∑distance(node_i,node_i+1)
obstacle＝∑distance(node_i,obstacle)
smooth＝∑angle(|node_i,node_i+1|)
wherein distance (node_i, node_i+1) refers to the distance between the i-th node and the i+1-th node on each candidate path, distance (node_i, obstacle) refers to the distance between the i-th node and the obstacle on each candidate path, angle (i node_i, node_i+1) refers to the absolute value of the difference between the angle of the i-th node and the angle of the i+1-th node on each candidate path;
According to the path length cost value length, the obstacle cost value obstacle and the smooth cost value smooth of each candidate path in the current path planning period, the total cost value C of each candidate path in the current path planning period is calculated, and the calculation formula is as follows:
C＝w1*length+w2*obstacle+w3*smooth
Wherein w 1、w2、w3 is a weight coefficient, respectively;
And determining a candidate path with the minimum total cost value C from the plurality of candidate paths in the current path planning period according to the total cost value C of each candidate path in the current path planning period, and taking the candidate path with the minimum total cost value C as the optimal path in the current path planning period.
2. The method for dynamic path planning based on the improved bi-directional RRT algorithm of claim 1, wherein said determining a new node by heuristic search during the current path planning period further comprises:
Calculating an angle theta 1 of a new node of the current tree and an angle theta 2 of a new node of another tree;
And determining a new node of the next path planning period according to the searching step length in the path direction along the angle theta 1 or the angle theta 2.
3. The improved bi-directional RRT algorithm-based dynamic path planning method of claim 1, further comprising:
1) Recording a total cost value C of an initial path planning period: recording the total cost value C in the initial five path planning periods, and calculating the average value as a reference value;
2) Monitoring the subsequent change of the total cost value C: continuously monitoring a total cost value C in each subsequent path planning period;
3) Triggering an automatic adjustment search strategy: comparing the total cost value C in the current path planning period with a reference value, triggering an automatic adjustment search strategy to reduce the search step length, and determining a new node through random search in the four subsequent path planning periods if the total cost value C in the current path planning period is larger than the reference value or the total cost value C in the four subsequent path planning periods continuously rises; if the total cost value C in the following four path planning periods still continuously rises or exceeds an emergency threshold C emergency after the automatic adjustment strategy is triggered, searching is directly carried out towards the offset reverse direction of the target point, and the searching step length is greatly increased;
4) Dynamically updating a reference value: taking the average value of the total cost value C of the latest five path planning periods as a reference value;
5) Continuing monitoring and feedback: after the search strategy is automatically adjusted, the change of the subsequent total cost value C is continuously monitored so as to ensure that the quality of path planning is continuously improved.
4. The method for dynamic path planning based on the modified bi-directional RRT algorithm of claim 1, wherein said smoothing and pruning the complete path from the start point to the end point to obtain a final path from the start point to the end point, further comprising:
smoothing the complete path from the starting point to the end point through a path smoothing algorithm to obtain a smooth path from the starting point to the end point;
Judging whether the nodes on the smooth path from the starting point to the end point are redundant points or not through a collision detection algorithm, and deleting the redundant points based on the principle of straight line path approximation so as to obtain a final path from the starting point to the end point.
5. The method for dynamic path planning based on the modified bi-directional RRT algorithm of claim 4, wherein said smoothing of said complete path from start point to end point by means of a path smoothing algorithm to obtain a smoothed path from start point to end point further comprises:
1) Selecting data points: taking N nodes on the complete path from the starting point to the end point as N data points to form a time sequence { x (1), x (2),. The number of the nodes, x (N) }, taking the data points in a window, namely { x (i-k), x (i-k+1),. The number of the nodes, x (i+k) }, wherein k is half of the size of the window, and rounding down, with each data point x (i) in the time sequence as a center;
2) Calculating the median: calculating the median of the data points in the window, namely, after the data points in the window are arranged according to the size, taking the value positioned in the middle position;
3) The median substitution is adopted: replacing the original data point x (i) with the calculated median value to obtain a smoothed data point;
4) Repeating the steps: with the movement of the dynamic window, a median smoothing operation is performed on all data points in the time series.
6. The method for dynamic path planning based on the modified bi-directional RRT algorithm of claim 4, wherein said determining by collision detection algorithm whether the node on the smooth path from the start point to the end point is a redundant point, further comprises:
For each pair of non-adjacent nodes on the smooth path, if the path between them is not affected by an obstacle, the intermediate node of each pair of non-adjacent nodes is marked as a redundant point.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410118463.1A CN117892896B (en) | 2024-01-29 | 2024-01-29 | Dynamic path planning method based on improved bidirectional RRT algorithm |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410118463.1A CN117892896B (en) | 2024-01-29 | 2024-01-29 | Dynamic path planning method based on improved bidirectional RRT algorithm |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117892896A CN117892896A (en) | 2024-04-16 |
| CN117892896B true CN117892896B (en) | 2024-08-23 |
Family
ID=90648857
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410118463.1A Active CN117892896B (en) | 2024-01-29 | 2024-01-29 | Dynamic path planning method based on improved bidirectional RRT algorithm |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117892896B (en) |
Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109542117A (en) * | 2018-10-19 | 2019-03-29 | 哈尔滨工业大学（威海） | Based on the submarine navigation device Rolling Planning algorithm for improving RRT |
| CN113858205A (en) * | 2021-10-25 | 2021-12-31 | 东南大学 | Seven-axis redundant mechanical arm obstacle avoidance algorithm based on improved RRT |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114115299B (en) * | 2022-01-25 | 2022-04-22 | 上海仙工智能科技有限公司 | Path planning method and device for smooth regression of mobile robot to given trajectory |
-
2024
- 2024-01-29 CN CN202410118463.1A patent/CN117892896B/en active Active
Patent Citations (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109542117A (en) * | 2018-10-19 | 2019-03-29 | 哈尔滨工业大学（威海） | Based on the submarine navigation device Rolling Planning algorithm for improving RRT |
| CN113858205A (en) * | 2021-10-25 | 2021-12-31 | 东南大学 | Seven-axis redundant mechanical arm obstacle avoidance algorithm based on improved RRT |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117892896A (en) | 2024-04-16 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111142522B (en) | Method for controlling agent of hierarchical reinforcement learning | |
| CN110083165B (en) | Path planning method of robot in complex narrow environment | |
| US10365110B2 (en) | Method and system for determining a path of an object for moving from a starting state to an end state set avoiding one or more obstacles | |
| CN108549385B (en) | Robot dynamic path planning method combining A-x algorithm and VFH obstacle avoidance algorithm | |
| CN107272679B (en) | Path planning method based on improved ant colony algorithm | |
| CN111610786A (en) | Mobile robot path planning method based on improved RRT algorithm | |
| CN113359746A (en) | Path planning method and device based on improved bidirectional RRT and Dijkstra fusion algorithm | |
| CN111290390A (en) | Intelligent ship path planning method based on longicorn stigma search | |
| CN110320919B (en) | Method for optimizing path of mobile robot in unknown geographic environment | |
| CN112229419A (en) | Dynamic path planning navigation method and system | |
| CN107357295B (en) | Path searching method and chip based on grid map and robot | |
| Hsu et al. | Path planning for mobile robots based on improved ant colony optimization | |
| CN113790729A (en) | Unmanned overhead traveling crane path planning method and device based on reinforcement learning algorithm | |
| Lee et al. | Cost based planning with RRT in outdoor environments | |
| CN110705803B (en) | Route planning method based on triangle inner center guide RRT algorithm | |
| Kiesel et al. | An effort bias for sampling-based motion planning | |
| CN116009527A (en) | Path planning algorithm based on dynamic scene structure expansion perception | |
| CN117892896B (en) | Dynamic path planning method based on improved bidirectional RRT algorithm | |
| CN117506893A (en) | Mechanical arm path planning and control method based on double planning and autonomous learning | |
| Xu et al. | UAV Local Path Planning Based on Improved Proximal Policy Optimization Algorithm | |
| Yu et al. | A novel automated guided vehicle (AGV) remote path planning based on RLACA algorithm in 5G environment | |
| CN117387649B (en) | Self-adaptive navigation method and system for uncertain environment robot with probability self-updating | |
| Ji et al. | Research on Path Planning of Mobile Robot Based on Reinforcement Learning | |
| CN118857305A (en) | Path planning method and system for artificial intelligent robot | |
| CN116679712B (en) | Efficient exploration decision-making method for indoor environment robot based on generalized voronoi diagram |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |