A heuristic bidirectional transition RRT robot arm obstacle avoidance motion planning method
By adopting a heuristic bidirectional transitional RRT method for obstacle avoidance motion planning of robotic arms, combined with bidirectional tree expansion and cost-driven search, the problems of low efficiency and difficulty in balancing constraints in path planning in high-dimensional environments are solved, generating high-quality, executable paths that are suitable for robotic arm motion planning in complex environments.
Patent Information
- Application Number
- CN202510525177.1
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2025-04-25
- Publication Date
- 2025-11-21
- Estimated Expiration
- 2045-04-25
AI Technical Summary
Existing path planning methods suffer from poor path quality, low planning efficiency, and unstable success rate in high-dimensional environments. They also struggle to accommodate multiple complex constraints, and are prone to local failures, especially in narrow passage environments, lacking multi-objective optimization capabilities.
A heuristic bidirectional transitional motion planning method for obstacle avoidance robotic arms is adopted, which combines bidirectional tree expansion, cost-driven search, adaptive annealing mechanism and heuristic strategy to generate more executable and better path motion trajectories. The path generation process is optimized by bidirectional search tree, heuristic distance strategy, local path collision detection and cost judgment.
It improves the efficiency and stability of path planning, generates high-quality paths, reduces redundancy, meets the execution requirements of robotic arms in complex environments, adapts to diverse performance needs, and is compatible with physical constraints in industrial applications.
Smart Images

Figure CN120116224B_ABST
Abstract
Description
TECHNICAL FIELD
[0001] The present application relates to the field of robot path planning, and relates to a heuristic bidirectional transition RRT robot arm obstacle avoidance motion planning method, in particular to a heuristic bidirectional transition rapidly-exploring random tree HB-TRRT motion planning method suitable for a high-dimensional complex environment, which can simultaneously consider path quality, execution efficiency and system constraint conditions, and is suitable for a high-dimensional joint space and a complex working environment. BACKGROUND
[0002] With the development of industrial automation and intelligent robot technology, the application of robot arms in high-precision and high-complexity tasks is becoming more and more widespread. Especially for robot arms with complex structure and high joint degrees of freedom, the path planning problem in narrow space or obstacle-dense environment has become an important bottleneck affecting the execution efficiency and stability of the task.
[0003] Traditional path planning methods, such as A* algorithm based on grid search or other deterministic search algorithms, are prone to "dimension disaster" when facing high dimension and complex constraints, not only the computational complexity rises sharply, but also it is difficult to balance path smoothness and executability. Under this background, path planning methods based on random sampling, such as Rapidly-exploring Random Tree, RRT, have become a research hotspot in recent years due to their rapid expansion capability in high-dimensional space.
[0004] RRT series algorithms have derived multiple variants, aiming to solve path feasibility, optimality and search efficiency, etc. For example, RRT-Connect uses a bidirectional tree structure to improve the speed of search connectivity, which is suitable for rapid exploration in large-scale space; RRT* constantly optimizes the path through a node reconnection mechanism, pursuing asymptotic optimality, but its convergence speed is slow and the computational cost is large; Informed-RRT* further introduces a heuristic ellipsoid sampling strategy based on RRT*, focusing on the potential optimal area to speed up the convergence speed.
[0005] However, the existing methods still have the following problems: strong randomness, large path redundancy, high post-processing cost; it is difficult to handle multiple complex constraints (such as joint limits, speed, energy consumption, etc.) at the same time; it is easy to fall into local failure in narrow channel environment, with unstable success rate; lack of multi-objective optimization ability, unable to measure the "quality" and "efficiency" of the path; therefore, there is an urgent need for a motion planning algorithm that takes into account efficiency, stability and multi-constraint adaptability to meet the path planning needs of high-dimensional robot arms in actual complex task scenarios. SUMMARY
[0006] In view of the above defects or improvement needs of the prior art, the present application provides a heuristic bidirectional transition RRT robot arm obstacle avoidance motion planning method, aiming to solve the problems of poor path quality, low planning efficiency, unstable success rate and difficulty in considering constraints in the high-dimensional environment of the existing path planning method. The method combines bidirectional tree expansion, cost-driven search, adaptive annealing mechanism and heuristic strategy, and can generate more executable and optimal motion trajectory while maintaining global search capability.
[0007] To achieve the above-mentioned purpose, according to one aspect of the present application, a heuristic bidirectional transition RRT robot arm obstacle avoidance motion planning method is provided, comprising the following steps:
[0008] S1: Taking the start node and the target node as the root node, two independent search trees are initialized respectively;
[0009] S2: On the basis of step S1, set global parameters;
[0010] S3: On the basis of step S2, start the main loop, and in each iteration, expand the current search tree once, if the expansion is successful and connects another tree, return the path immediately; otherwise, enter the next round of loop;
[0011] S4: Select the search tree to be expanded based on the heuristic distance strategy of the last successful expansion node in step S3;
[0012] S5: Uniformly sample in the configuration space, randomly generate a sample node and find its nearest neighbor node, and generate a new node from the neighbor node towards the sample node;
[0013] S6: Perform local path collision detection on the new node generated in step S5, if there is a collision, discard the new node generated in step S5 and jump out of the current loop; if there is no collision, continue to perform cost judgment;
[0014] S7: Perform cost judgment on the new node detected without collision in step S6 to determine whether to accept the new node expansion;
[0015] S8: Add the new node accepted in step S7 to the current search tree and try to connect with another tree;
[0016] S9: Terminate the algorithm when the two trees are connected successfully in step S8 or the maximum number of iterations is reached;
[0017] S10: Generate a motion trajectory using the cubic spline interpolation method in step S9.
[0018] Preferably, the global parameters in step S2 include temperature parameters, upper and lower temperature limits, maximum number of iterations, expansion step size, node connectivity distance threshold, cost function weight parameters, and configuration space and obstacle modeling.
[0019] Wherein, the temperature parameter T = T init Initial temperature T init The initial gradient range of the cost function C(q) is empirically set, and its formula is as follows:
[0020]
[0021] in, This represents the minimum path cost in the initial stage. is the maximum value of the path in the initial stage, and k is the adjustment coefficient.
[0022] As a preferred option, the heuristic distance strategy selection principle in step S4 is: calculate the extended node q′ of the starting tree. start To the target tree T goal The distance d of the nearest node s d g The extended node q′ of the target tree goal To the starting tree T start The distance d of the nearest node g When d s <d g At that time, expand the starting tree T start Conversely, expand the target tree T goal The calculation formula is as follows:
[0023] d s =D(q′) start ,N(T goal ,q′ start ))
[0024] d g =D(q′) goal ,N(T start ,q′ goal ))
[0025] Among them, T start and T goal Let q′ represent the starting tree and the target tree, respectively. start q′ is the newly generated extended node of the starting tree. goal For the newly generated extended nodes of the target tree, N(T) goal ,q′ start ) represents the target tree T goal With the extended node q′ start The nearest node between N(T) start ,q′ goal ) represents the starting tree Tstar the nearest node between the extended node q' and q goal ; D(a, b) represents the Euclidean distance between two nodes.
[0026] As a preferred, step S5 specifically includes the following steps:
[0027] S51: uniformly sampling in the configuration space, randomly generating a sampling node q rand ;
[0028] S52: in the current search tree, finding the nearest neighbor node q rand of the sampling node q near obtained by step S51.
[0029] S53: expanding the neighbor node q near obtained by step S52 in the direction from the sampling node q rand to generate a new node q new , whose formula is:
[0030]
[0031] wherein, δ is a step parameter.
[0032] As a preferred, the local path collision detection method executed in step S6 is: using the obstacle model to determine whether the path segment from the neighbor node q near to the new node q new collides, if the path collides, discarding the new node q new and jumping out of the current loop; if the path does not collide, continuing to perform the cost judgment.
[0033] As a preferred, step S7 specifically includes the following steps:
[0034] S71: for the new node q new that passes the collision detection, calculating its relative path cost, and the cost function formula is:
[0035] C(q) = w l ·L(q) + w t ·T(q)
[0036] wherein, L(q) is the path length, T(q) is the execution time based on the maximum speed of each joint, w l and w t are adjustable weight parameters.
[0037] L(q) = ∑||q i+1 -q i ||
[0038]
[0039] where q i+1 is the position of the i-th path node, i is the position of the i+1-th path node, is the position of the path node on the j-th joint, is the maximum velocity of the j-th joint.
[0040] S72: Calculate the cost increment AC between the new and old nodes, which is given by:
[0041] AC = C(q new ) - C(q near )
[0042] where C(q new ) is the path cost value calculated for the new node q new , and C(q near ) is the path cost value calculated for the neighboring node q near ;
[0043] S73: Use the improved Metropolis criterion to make a cost transition decision: if AC≤0, it means that the path cost is reduced, and the expanded new node q new is accepted; if AC>0, calculate the transition probability P, which is given by:
[0044] P = exp(-AC / T)
[0045] where T is the temperature;
[0046] When P>0.5, the expanded new node q new is accepted.
[0047] As a preferred embodiment, step S8 specifically comprises the following steps:
[0048] S81: Add the new node q new accepted in step S7 to the current search tree, and add a directed edge between the new node q new and the parent node q near , to construct a new search path;
[0049] S82: Find the child node q new closest to the new node q connect in the other search tree, and calculate the Euclidean distance between them. If the distance D(q new , q connnect ) is less than the preset node connectivity distance threshold ε, it is considered that the two trees have connectivity potential, and a connection attempt is made. If the connection is successful, the path is returned through the path extraction function;
[0050] S83: Adjust the temperature according to the adaptive annealing mechanism, if the expansion is rejected, i.e. not passed the improved Metropolis criterion, then perform a temperature raising operation on the temperature T:
[0051] T←T·2 α ,α∈(0,1]
[0052] If the expansion is accepted, then perform a temperature lowering operation on the temperature T according to the current cost transition amplitude:
[0053]
[0054] Wherein, C max and C min are the maximum and minimum path costs in the current tree node respectively.
[0055] As a preference, the method of attempting connection in step S82 is to call a piecewise connection function, attempting connection piece by piece with a step size of δ; and each piece needs to pass the collision detection of step S6 and the cost judgment of step S7.
[0056] As a preference, the specific method of the piecewise connection function to construct the connection path is:
[0057] (1) Calculate the vector direction from node q connect to new node q new , and convert it to a unit vector The formula is:
[0058]
[0059] (2) Take δ as the step size, and the intermediate node q step generated in each round is obtained from the intermediate node q last generated in the last round, and the formula is:
[0060]
[0061] (3) The intermediate node q step passes the collision detection of step S6 and the cost judgment of step S7, if all path segments pass, it is considered to be connected successfully, and the intermediate node q step is added to the other search tree one by one to form a double-tree closed structure; if any path segment fails to pass the collision detection or cost judgment during the connection process, the connection attempt fails, and the failure is returned;
[0062] (4) Backtrack from the new node q new to the starting node q start , and backtrack along the parent node chain in the starting tree T start to form a starting path segment; backtrack from the node q connect to the target node qgoal Along the target number T goal The parent node chain is traced back in reverse to form a target path segment; the starting path segment and the target path segment are spliced together to form a complete, collision-free, and cost-acceptable execution path, which is then returned as the output of this planning.
[0063] Preferably, the cubic spline interpolation method in step S10 involves fitting discrete path points, with each interval corresponding to an independent cubic polynomial, and the generated trajectory function is:
[0064] q(t)=a i +b i (tt i )+c i (tt i ) 2 +d i (tt i ) 3 ,t∈[t i ,t i+1 ]
[0065] Where q(t) represents the trajectory position of the joint at time t; t i ,t i+1 a represents the start and end times of the path segment. i b i c i d i These are the fitting coefficients for each interpolation interval.
[0066] Through the above technical solutions, the HB-TRRT motion planning method proposed in this invention can effectively improve the efficiency and stability of path planning in high-dimensional space. Compared with traditional RRT-like algorithms, this method has stronger path optimization capabilities and search robustness when facing complex obstacles and high-cost regions. It can significantly reduce invalid searches and redundant paths, and generate smoother, safer, and constraint-satisfying executable paths, making it suitable for motion planning tasks of robotic arms in complex environments.
[0067] In summary, the technical solutions conceived by this invention have the following beneficial effects compared with the prior art:
[0068] 1. Maintain a high search success rate within a limited planning time, especially demonstrating stable connectivity in typical constrained scenarios such as narrow channels.
[0069] 2. The generated path has higher quality and stability, with smaller fluctuations in path length and a reasonable distribution of node numbers, resulting in reduced trajectory redundancy, which is beneficial for subsequent trajectory execution and control processing.
[0070] 3. The cost function comprehensively considers path length and execution time, supports flexible trade-off between path compactness and task response speed, and thus adapts to diversified performance requirements such as energy saving or time optimization.
[0071] 4. The actual execution limit of the robot arm is considered in the extension and connection process, and the physical constraint conditions such as joint limit, maximum speed and maximum acceleration commonly used in industrial applications are compatible, so that the generated path has actual executability.
[0072] 5. The weight parameter and annealing temperature adjustment mechanism introduced in the algorithm have good adjustability, support adjusting path cost weight distribution and cost transition acceptance strategy according to different application tasks, and thus improve the adaptability and engineering usability of the algorithm. BRIEF DESCRIPTION OF DRAWINGS
[0073] Figure 1 The figure is a schematic diagram of the extension process of the HB-TRRT algorithm.
[0074] Figure 2 The figure is a complete flowchart of the HB-TRRT algorithm.
[0075] Figure 3 The figure is a schematic diagram of the posture of the motion planning task of the embodiment robot arm.
[0076] Figure 4 The figure is a comparison diagram of the Cartesian space path of the end of the robot arm in the motion planning task.
[0077] Figure 5 The figure is a position, velocity and acceleration trajectory diagram of the moving joint one of the embodiment robot arm in the joint space.
[0078] Figure 6 The figure is a position, velocity and acceleration trajectory diagram of the rotating joint seven of the embodiment robot arm in the joint space. DETAILED DESCRIPTION
[0079] In order to make the purpose, technical scheme and advantages of the present application clearer, the present application is further described in detail below in combination with the drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present application and do not limit the present application. In addition, the technical features involved in each embodiment of the present application described below can be combined with each other as long as they do not conflict with each other.
[0080] The present application provides a heuristic bidirectional transition RRT robot arm obstacle avoidance motion planning method, which combines bidirectional search tree structure, cost controlled path extension mechanism and heuristic tree extension strategy through HB-TRRT algorithm, and is suitable for realizing path planning of the robot arm in the environment with high joint space dimension and complex constraint conditions. The following will be described in combination with the algorithm extension processFigure 1 Algorithmic flow Figure 2 The implementation flow of the method is described step by step, including the following steps:
[0081] S1: Take the start node and the target node as the root nodes, and initialize two independent search trees respectively;
[0082] Initially, the start node q start and the target node q goal are respectively taken as the root nodes to initialize two independent fast exploration random trees T start and T goal , that is, the start tree T start is constructed with the start node q start , and the target tree T goal is constructed with the target node q goal . The introduction of bidirectional search trees is to avoid the situation of low search efficiency and path detour in high-dimensional space caused by one-way expansion. The two trees grow from both ends at the same time and try to meet in the middle area, greatly improving the success rate of search connectivity and the overall path compactness.
[0083] S2: On the basis of step S1, set global parameters, including temperature parameters, temperature upper and lower limits, maximum iteration number, expansion step, node connectivity distance threshold, cost function weight parameters, and configuration space and obstacle modeling;
[0084] wherein the temperature parameter T = T init , the initial temperature T init is empirically set according to the initial gradient range of the cost function C(q), and its formula is:
[0085]
[0086] wherein, the minimum value of the path cost in the initial stage, the maximum value of the path cost in the initial stage, and k is the adjustment coefficient.
[0087] The temperature upper and lower limits T max and T min are set to prevent infinite temperature changes, and the upper and lower limits are used to limit the range of temperature updates.
[0088] The maximum iteration number I max controls the termination condition of the algorithm in the case of no solution. This parameter determines the maximum number of times the tree structure is allowed to expand, and must be reasonably set according to the complexity of the task.
[0089] The expansion step δ controls the sampling node q near from the neighbor node q randMaximum length of expansion. Too large may cause the algorithm to go beyond the obstacles, too small may slow down the convergence.
[0090] Node connection distance threshold ε. When connecting two tree structures, if the distance between two nodes is less than ε, it is considered to be successfully connected. This threshold determines the accuracy of the termination criterion.
[0091] Cost function weight parameter w l , w t , the relative weight of path length and execution time in the control path cost function. According to the application requirements, such as energy priority or speed priority, it can be flexibly set.
[0092] Configuration space and obstacle modeling, define the joint space range of the robot arm, the speed and acceleration limit of each joint, and the geometric model of the obstacle, which is used for subsequent collision detection and path constraint.
[0093] S3: On the basis of step S2, start the main loop, loop from 1 to I max , in each iteration, expand the current search tree, if the expansion is successful and connects another tree, return the path immediately; otherwise, enter the next loop;
[0094] S4: Select the search tree to be expanded based on the heuristic distance strategy of the last successful expansion node in step S3;
[0095] To improve search efficiency and inter-tree connectivity probability, the following heuristic distance strategy is used to select the current expansion tree, the selection principle is: calculate the distance d start of the expansion node q′ goal of the starting tree to the nearest node in the target tree T s , d g represents the distance of the expansion node q′ goal of the target tree to the nearest node in the starting tree T start , d g , when d s <d g , expand the starting tree T start , otherwise expand the target tree T goal , the calculation formula is:
[0096] d s =D(q′ start ,N(T goal ,q′ start ))
[0097] d g =D(q′ goal ,N(T start ,q′ goal ))
[0098] Among them, T start and T goal Let q′ represent the starting tree and the target tree, respectively. start q′ is the newly generated extended node of the starting tree. gial For the newly generated extended nodes of the target tree, N(T) goal ,q′ start ) represents the target tree T goal With the extended node q′ start The nearest node between N(T) start ,q′ goal ) represents the starting tree T star With the extended node q′ goal The nearest node between two nodes; D(a,b) represents solving for the Euclidean distance between the two nodes.
[0099] This strategy dynamically determines the proximity between trees, making the expansion direction of the trees more coordinated.
[0100] S5: Perform uniform sampling in the configuration space, randomly generate sampling nodes and find their nearest neighbor nodes, and generate new nodes from the neighbor nodes toward the sampling nodes;
[0101] S51: Perform uniform sampling in the configuration space and randomly generate sampling nodes q. rand ;
[0102] This operation is a key part of the RRT series of algorithms, used to improve global search coverage and sample node q. rand It is randomly generated from the entire configuration space, and its purpose is to improve global exploration capabilities and avoid getting stuck in local areas.
[0103] S52: In the current search tree, find the sampling node q obtained in step S51. rand nearest neighbor node q near ;
[0104] By minimizing the distance function D(q) rand The distance metric is typically Euclidean distance, but can also be extended to weighted joint distance or heuristic distance. The goal is to find the closest sampling node q in the search tree. rand The existing neighboring node q near This serves as the starting point for the next stage of expansion.
[0105] S53: Neighbor node q obtained from step S52 near To sampling node q rand Direction, expand to generate new node q new Its formula is:
[0106]
[0107] Wherein, δ is a step parameter, controlling the expansion step, avoiding once across the obstacle region, ensuring the local continuity and controllability of the expansion process. The interpolation method retains the information of the sampling direction, while enhancing the density of the tree structure.
[0108] S6: Perform local path collision detection on the new node generated in step S5, if there is a collision, discard the new node generated in step S5 and jump out of the current loop; if there is no collision, continue to perform cost judgment;
[0109] The method for performing local path collision detection is: using the obstacle model to determine whether the path segment from the neighbor node q near to the new node q new collides, if the path collides, discard the new node q new and jump out of the current loop; if the path does not collide, continue to perform cost judgment.
[0110] S7: Perform cost judgment on the new node detected in step S6 without collision, and judge whether to accept the new node expansion;
[0111] S71: For the new node q new that passes the collision detection, calculate its relative path cost, and the cost function formula is:
[0112] C(q)=w l ·L(q)+w t ·T(q)
[0113] Wherein, L(q) is the path length, T(q) is the execution time based on the maximum speed of each joint, w l and w t are adjustable weight parameters;
[0114] L(q)=∑||q i+1 -q i ||
[0115]
[0116] Wherein, q i+1 , q i are the i-th and i+1-th path nodes, is the position of the path node on the j-th joint, is the maximum speed of the j-th joint.
[0117] S72: Calculate the cost increment ΔC between the new and old nodes, and the formula is:
[0118] ΔC=C(q new )-C(q near )
[0119] Wherein, C(q) new ) is the new node q new The calculated path cost, C(q) near ) is a neighboring node q near The calculated path cost;
[0120] S73: Use the improved Metropolis criterion to determine cost transitions: if ΔC≤0, it indicates a decrease in path cost, and the new node q is accepted for expansion. new If ΔC > 0, calculate the transition probability P, which is given by the following formula:
[0121] P = exp(-ΔC / T)
[0122] Where T is temperature;
[0123] When P > 0.5, accept the expansion of the new node q. new .
[0124] S8: Add the new node accepted in step S7 to the current search tree and attempt to connect it to another tree;
[0125] S81: The new node q accepted in step S7 new Add to the current search tree, and in the new node q new With parent node q near Add a directed edge between them to construct a new search path;
[0126] S82: Find the new node q in another search tree. new The closest slave node q connect Calculate the Euclidean distance between the two. If the distance D(q) new ,q connnect If the distance between two nodes is less than the preset node connectivity threshold ε, then the two trees are considered to have connectivity potential, and a connection attempt is made. That is, the segmented connection function is called, and the connection is attempted segment by segment with a step size of δ. Each segment must pass the collision detection in step S6 and the cost judgment in step S7. If the connection is successful, the path is returned through the path extraction function.
[0127] The specific method for constructing the join path using the segmented join function is as follows:
[0128] (1) Calculate the value of node q connect To the new node q new The vector direction is transformed into a unit vector. The formula is:
[0129]
[0130] (2) Take δ as the step size, and generate the intermediate node q step The intermediate node q generated by the last round last Get, the formula is:
[0131]
[0132] (3) The intermediate node q step Through the collision detection of step S6 and the cost judgment of step S7, if all the path segments pass, it is considered to be connected successfully, and the intermediate node q step is added to the other search tree one by one to form a double-tree closed structure; if any path segment fails to pass the collision detection or the cost judgment during the connection process, the connection attempt fails, and the failure is returned;
[0133] (4) From the new node q new , backtrack to the starting node q start , and backtrack along the parent node chain in the starting tree T start to form a starting path segment; from the node q connect , backtrack to the target node q goal , and backtrack along the parent node chain in the target tree T goal to form a target path segment; the starting path segment and the target path segment are spliced head to tail to form a complete, collision-free and acceptable cost execution path, which is returned as the output of this planning.
[0134] S83: Adjust the temperature according to the adaptive annealing mechanism, if the expansion is rejected, that is, it does not pass the improved Metropolis criterion judgment, then perform a temperature raising operation on the temperature T:
[0135] T←T·2 α ,α∈(0,1]
[0136] If the expansion is accepted, then perform a temperature lowering operation on the temperature T according to the current cost transition amplitude:
[0137]
[0138] Where C max and C min are the maximum and minimum path costs in the current tree node, respectively.
[0139] S9: If the connection of the two trees is successful or the maximum number of iterations is reached, terminate the algorithm;
[0140] If the two trees have been successfully connected, end the algorithm and return the feasible path; if the current iteration number has reached the upper limit I max , the algorithm returns failure, indicating that a feasible path cannot be found in the current task environment.
[0141] S10: generating the motion trajectory by using a cubic spline interpolation method according to step S9.
[0142] The cubic spline interpolation method is to fit discrete path points, and each interval corresponds to an independent cubic polynomial, and the generated trajectory function is:
[0143] q(t) = a i +b i (t-t i )+c i (t-t i ) 2 +d i (t-t i ) 3 ,t∈[t i ,t i+1 ]
[0144] Wherein, q(t) represents the trajectory position of the joint at time t; t i ,t i+1 are the start and end times of the path segment, a i , b i , c i , d i are the fitting coefficients of each interpolation interval.
[0145] The cubic spline interpolation method can ensure that the path passes through all nodes while realizing the continuity of the first-order derivative velocity and the second-order derivative acceleration, and realizing the smooth processing of the trajectory of the robot arm.
[0146] To further verify the applicability and stability of the HB-TRRT path planning algorithm in high-dimensional constrained space, the following selects a certain mechanical arm path planning task as an example embodiment. The practical performance of the method in the high constraint complex scene is explained:
[0147] The example mechanical arm is a 7-degree-of-freedom mechanical arm, including 1 linear movement joint and 6 rotary joints, and has typical high degree of freedom redundancy characteristics. The example scene is a three-dimensional simulation environment with obvious narrow channel structure, representing the typical working condition of the mechanical arm entering the narrow structure to perform maintenance or assembly tasks, such as Figure 3 The path planning goal is to move from the initial retracted posture to the fully expanded working posture, and needs to avoid walls and channel boundaries and meet the overall constraints.
[0148] The simulation environment is built based on the ROS MoveIt and Gazebo platforms, and the dynamics model of the mechanical arm includes complete joint position limits, maximum speed and maximum acceleration constraints, and the constraint conditions are shown in Table 1:
[0149] Table 1
[0150]
[0151] RRT, RRT*, RRT-Connect, TRRT and the HB-TRRT method of the present application were used to plan paths for this task, with the upper limit time set to 1120s. Each algorithm was independently executed 10 times to reduce the influence of chance, and the average planning time, standard deviation and path success rate were calculated. The experimental data is shown in Table 2 below:
[0152] Table 2
[0153]
[0154] Further, 5 successful planning paths were selected from the successful results of each algorithm, and the number of path nodes, joint space path length and Cartesian space path length of the end effector were evaluated to compare path redundancy and path smoothness. The experimental data is shown in Table 3 below:
[0155] Table 3
[0156]
[0157] The results of the example show that HB-TRRT has the following advantages in high-dimensional constrained scenarios: maintaining a 100% success rate, avoiding the drawbacks of traditional algorithms that are prone to sampling failure in narrow environments; the average planning time and path length stability are better than other methods, with a small fluctuation range, showing stronger execution certainty; the number of path nodes is less, indicating lower redundancy, suitable for trajectory tracking execution; and it meets the comprehensive executability requirements of path smoothness, collision-free, and limit violation-free.
[0158] Figure 4 The figure shows the end effector trajectory distribution of the paths generated by each algorithm in Cartesian space. It can be seen that the paths generated by HB-TRRT are the shortest on average, with the smallest trajectory fluctuation, high similarity between multiple execution results, good reproducibility and stability.
[0159] Figure 5 With Figure 6 The figures show the execution trajectories of the paths generated by the HB-TRRT method on joint one and joint seven, respectively, where the red dashed line represents the speed and acceleration constraint boundary. As can be seen from the figures, all paths do not exceed the boundary, and there is no mutation or shock, proving that the method has good industrial usability.
[0160] In summary, the HB-TRRT path planning method introduced in the application solves the key problems of strong randomness of path in high-dimensional space, difficulty in processing constraints, and low connectivity efficiency by introducing a bidirectional search structure, a cost-controlled path expansion mechanism, and a heuristic expansion strategy. While maintaining search efficiency, a stable, smooth, and highly executable path can be generated. The method proposed in the application has good adaptability and implementability in the execution of complex operation tasks by industrial robots, especially in scenes with spatial constraints and multiple obstacle restrictions, and has practical application value.
[0161] Those skilled in the art will easily understand that the above description is only a preferred embodiment of the present application, and is not intended to limit the present application, and any modifications, equivalent replacements and improvements made within the spirit and principles of the present application shall be included in the protection scope of the present application.
Claims
1. A heuristic bidirectional transition RRT robot arm obstacle avoidance motion planning method, characterized in that, The method comprises the following steps: S1: initializing two independent search trees respectively with the start node and the target node as root nodes; S2: setting global parameters based on step S1; S3: starting a main loop based on step S2, in each iteration, expanding the current search tree, if the expansion is successful and connects another tree, returning the path immediately, otherwise, entering the next loop; S4: selecting the search tree to be expanded based on the heuristic distance strategy of the last expanded node in step S3; S5: uniformly sampling in the configuration space, randomly generating a sampling node and finding its nearest neighbor node, and generating a new node from the neighbor node to the sampling node; S6: performing local path collision detection on the new node generated in step S5, if the path has collision, discarding the new node generated in step S5 and jumping out of the current loop, if the path has no collision, continuing to perform cost judgment; S7: performing cost judgment on the new node detected in step S6 without collision, and judging whether to accept the new node expansion; S8: adding the new node accepted in step S7 to the current search tree and trying to connect with another tree; S9: terminating the algorithm when the two trees are connected successfully in step S8 or the maximum number of iterations is reached; S10: generating a motion trajectory using a cubic spline interpolation method in step S9.
2. The heuristic bidirectional transition RRT manipulator obstacle avoidance motion planning method of claim 1, wherein, The global parameters in step S2 include temperature parameters, upper and lower limits of temperature, maximum number of iterations, expansion step, node connection distance threshold, cost function weight parameters, and configuration space and obstacle modeling, where the temperature parameter T = T init , initial temperature T init is empirically set according to the initial gradient range of the cost function C(q), and its formula is: wherein, is the minimum value of the initial stage path cost, is the maximum value of the initial stage path, k is an adjustment factor.
3. The heuristic bidirectional transition RRT manipulator obstacle avoidance motion planning method of claim 1, wherein, The heuristic distance strategy selection principle in step S4 is: calculate the expanded node q′ of the starting tree. start To the target tree T goal The distance d of the nearest node s d g The extended node q′ of the target tree goal To the starting point tree T start The distance d of the nearest node g When d s <d g At that time, expand the starting tree T start Conversely, expand the target tree T goal The calculation formula is as follows: d s = D(q start , N(T goal , q start )) d g = D(q goal , N(T start , q goal )) where T start and T goal represent the source tree and the target tree respectively, q' start is a newly generated extended node in the source tree, q' goal is a newly generated extended node in the target tree, N(T goal , q' start ) represents the nearest node between the target tree T goal and the extended node q' start , N(T start , q' goal ) represents the nearest node between the source tree T start and the extended node q' goal ; and D(a, b) represents the Euclidean distance between two nodes.
4. The heuristic bidirectional transition RRT manipulator obstacle avoidance motion planning method of claim 1, wherein, Step S5 specifically comprises the following steps: S51: uniformly sampling in the configuration space, randomly generating a sampling node q rand ; S52: In the current search tree, find the sample node q obtained by step S51 rand the nearest neighbor node q near ; S53: neighbor node q obtained from step S52 near to the sampling node q rand direction, the expansion generates a new node q new whose formula is: Wherein, δ is the step parameter.
5. The heuristic bidirectional transition RRT manipulator obstacle avoidance motion planning method of claim 1, wherein, The local path collision detection method executed in step S6 is: judging whether the path segment from the neighbor node q near to the new node q new collides or not by using the obstacle model, if the path collides, discarding the new node q new and jumping out of the current loop; if the path does not collide, continuing to make the cost judgment.
6. The heuristic bidirectional transition RRT manipulator obstacle avoidance motion planning method of claim 1, wherein, Step S7 specifically comprises the following steps: S71: For the new node q by collision detection new , its relative path cost is calculated, and the cost function formula is: C(q) = w l • L(q) + w t • T(q) where L(q) is the path length, T(q) is the execution time based on the maximum velocity of each joint, w k and w t are adjustable weight parameters; L(q) =∑||q i+1 -q i || where q i+1 , q i is the i-th and i+1-th path node, is the position of the path node on the j-th joint, the maximum velocity of the j-th joint; S72: calculate the cost increment ΔC between the new and old nodes, the formula is: AC = C(q new ) - C(q near ) Wherein, C(q) new ) is the new node q new The calculated path cost, C(q) near ) is a neighboring node q near The calculated path cost; S73: Cost transition judgment using improved Metropolis criterion: if ΔC≤0, it indicates that the path cost is reduced, and the new node q is accepted new ; if ΔC>0, the transition probability P is calculated, and the formula is: P = exp(-ΔC / T) Wherein, T is the temperature; When P > 0.5, accept extended new node q new .
7. The heuristic bidirectional transition RRT manipulator obstacle avoidance motion planning method of any one of claims 1-6, wherein, Step S8 specifically comprises the following steps: S81: Accept the new node q new Add the current search tree, and in the new node q new Add a directed edge between the parent node q near and the new node q, and build a new search path; S82: Find another search tree in which to search for a new node q new The closest distance from node q connect , calculate the Euclidean distance between them, if the distance D(q new , q connnect ) is less than the preset node connectivity distance threshold ε, it is considered that the two trees have connectivity potential, and connection attempt is made, if the connection is successful, the path is returned through the path extraction function.
8. The heuristic bidirectional transition RRT manipulator obstacle avoidance motion planning method of claim 7, wherein, The method of trying to connect in step S82 is to call a segmented connection function, trying to connect in segments with δ as the step, and each segment needs to pass the collision detection in step S6 and the cost judgment in step S7; Further comprising step S83: adjusting the temperature according to the adaptive annealing mechanism, if the expansion is rejected, i.e. not passing the improved Metropolis criterion judgment, then the temperature T is raised: T <- T - 2 α , a e (0, 1] If the expansion is accepted, the temperature T is lowered according to the current cost transition amplitude: where C max and C min are the maximum and minimum path costs in the current tree node, respectively.
9. The heuristic bidirectional transition RRT manipulator obstacle avoidance motion planning method of claim 8, wherein, The specific method of the segmented connection function for constructing the connection path is: (1) Calculate the vector direction from node q connect to new node q new , and convert it to a unit vector , whose formula is: (2) Intermediate nodes q generated in each round with a step size of δ step Intermediate nodes q generated by the previous round last The formula is: (3) Intermediate node q step Through the collision detection of step S6 and the cost judgment of step S7, if the path segment passes through all, it is considered as a successful connection, and the intermediate node q step One by one, add another search tree to form a double-tree closed structure; If any segment of the path fails to pass the collision detection or cost judgment during the connection process, the connection attempt fails and returns to failure; (4) from the new node q new Backtrack to the start node q start , reverse along the parent node chain in the start tree T start , forming a start path segment; from the node q connect Backtrack to the target node q goal , reverse along the parent node chain in the target tree T goal , forming a target path segment; concatenate the start path segment and the target path segment head to tail as a complete, collision-free and acceptable cost execution path, return as the output of this planning.
10. The heuristic bidirectional transition RRT manipulator obstacle avoidance motion planning method of any one of claims 7-9, wherein, The cubic spline interpolation method in step S10 is to fit the discrete path points, each interval corresponds to an independent cubic polynomial, and the generated trajectory function is: q(t) = a i + b i (t - t i ) + c i (t - t i ) 2 + d i (t - t i ) 3 , t e [t i , t i+1 ] where q(t) represents the trajectory position of the joint at time t; t i ,t i+1 are the start and end times of the path segment, a i , b i , c i , d i are the fitting coefficients for each interpolation interval.
Citation Information
Patent Citations
Robot path planning method and device
CN109269507A
Mechanical arm on-line obstacle avoidance movement planning method
CN110228069A
