A Trajectory Planning Method for RRT Robotic Arms Based on Non-obstacle Spatial Probabilistic Potential Field Sampling

Abstract

This invention proposes a trajectory planning method for RRT robotic arms based on probabilistic potential field sampling in non-obstacle space. The method includes generating sampling points in non-obstacle space, filtering sampling points based on whether they fall into obstacle space to avoid generating redundant paths and unnecessary path collision detection. The planning method also includes a greedy sampling strategy for the probabilistic potential field, which, while ensuring the algorithm's probability completeness, assigns a target-oriented nature to the algorithm with a preset probability to accelerate path generation. Furthermore, it uses the interaction of gravitational and repulsive fields to reduce path collision detection and generate a more optimized path direction. The planning method also includes a redundant node removal strategy, which optimizes the path generated by the algorithm's backtracking, removing redundant nodes to improve the generated path. Finally, it uses interpolation polynomials to smooth the optimized path, making it more consistent with actual robotic arm motion. This invention can generate smooth paths more efficiently and has significant industrial application value.

Description

Technical Field

[0001] This invention relates to the field of robotic arm technology, and in particular to the RRT robotic arm trajectory planning method based on non-obstacle space probability potential field sampling. Background Technology

[0002] With the continuous development of artificial intelligence and intelligent manufacturing technologies, future industrial manufacturing will inevitably be highly intelligent, information-based, and characterized by human-machine collaboration. The working scenarios faced by robotic arms will also be complex and unpredictable. As the application environment of industrial robotic arms becomes increasingly complex, the role of obstacle avoidance path planning becomes increasingly important. Obstacle avoidance path planning refers to the ability of a robotic arm to plan a collision-free path within a limited time when obstacles exist in the workspace.

[0003] The existing technology and its problems are as follows:

[0004] 1. The trajectory planning algorithm based on random sampling - Rapidly-exploring RandomTrees (RRT) is a trajectory planning method that is widely applicable and probabilistically complete in two-dimensional and high-dimensional spaces, and is widely used in obstacle avoidance path planning for robotic arms.

[0005] 2. The RRT-Connect algorithm grows two random trees simultaneously from the initial and target points, exhibiting excellent search characteristics. Compared to the RRT algorithm, it significantly improves search speed and efficiency. The asymptotically optimal RRT* algorithm guarantees computational complexity and an asymptotically optimal solution. The Informed-RRT* algorithm can return to the asymptotically optimal solution faster. Due to the randomness of the sampling points generated by the RRT algorithm, it leads to blind sampling in the workspace, resulting in excessively high time costs for planning feasible paths. It also generates a large number of redundant nodes, leading to high space costs and significant randomness.

[0006] 3. The Artificial Potential Field (APF) method performs well in robot workspaces with few or no obstacles. However, in workspaces with complex obstacles, the attractive and repulsive forces acting on the robot in the artificial potential field can easily cancel each other out, causing the robot to fall into local minima.

[0007] 4. The invention patent CN202210671495.5 uses a target bias method to make RRT more purposeful while maintaining probabilistic completeness, shortening the search time and using a triangular pruning method to optimize the path. However, it samples the entire workspace and does not consider obstacle information during the target bias process, which results in a large number of invalid sampling points and consumes a lot of time for collision detection.

[0008] 5. The paper "Robotic Arm Obstacle Avoidance Planning Combining RRT and Artificial Potential Field Method" combines the RRT algorithm with artificial potential field. When the artificial potential field gets stuck in a local minimum, virtual target points are generated by RRT sampling to escape the local minimum.

[0009] 6. Invention patent CN201810008810.X addresses the problem that artificial potential field methods are prone to getting trapped in local minima by adding virtual obstacles and improving the potential field function, enabling the robotic arm to escape local minima and complete path planning.

[0010] 7. Invention patent CN202210973726.8 addresses the local minima problem by setting up virtual target points to complete path planning. However, the path it finds is not optimal, and after getting stuck in a local minima, the robotic arm will experience high oscillations in this area, causing strong physical impact on the robotic arm. Summary of the Invention

[0011] This invention proposes an RRT robotic arm trajectory planning method based on non-obstacle space probability potential field sampling, which can generate smooth paths more efficiently and has very high industrial application value.

[0012] The present invention adopts the following technical solution.

[0013] The RRT robotic arm trajectory planning method based on non-obstacle space probabilistic potential field sampling is used for multi-link robotic arms with multiple joints. It uses RRT combined with artificial potential field to perform global path planning in non-obstacle space. The planning method includes generating sampling points based on non-obstacle space, that is, filtering sampling points by whether they fall into obstacle space, avoiding the generation of redundant paths and unnecessary path collision detection.

[0014] The planning method also includes a probabilistic potential field greedy sampling strategy, which, under the premise of ensuring the completeness of the algorithm probability, gives the algorithm a target orientation with a preset probability to accelerate path generation, and reduces path collision detection and generates a relatively better path direction through the interaction of gravitational and repulsive fields.

[0015] The planning method also includes a redundant node removal strategy, which optimizes the path generated by the algorithm backtracking, removes redundant nodes in the path, and makes the generated path better; and smooths the optimized path by interpolation polynomial, so that the path is more in line with the actual movement of the robotic arm to reduce friction loss during violent movements.

[0016] The planning method includes the following steps;

[0017] Step S1: Mathematical modeling of the robotic arm: Describe the pose of the robotic arm and establish the link coordinate system of the robotic arm to obtain the DH model of the robotic arm;

[0018] Step S2: Initialize to perform global path planning, specifically: initialize tree T in the workspace, given an initial node q. start =[x start y start , z start ], target node q goal =[x goal y goal , z goal [ ], Extended fixed step size step, maximum number of iterations max, obstacle space Ω obs And the target point error allowable threshold thr;

[0019] Step S3: Execute the probabilistic potential field greedy sampling strategy, that is, use the non-obstacle space probabilistic potential field sampling to generate sampling points: Let the random probability threshold be P. When the generated random probability p is less than P, the algorithm performs random sampling to obtain random sampling points q. rand =[x rand y rand， z rand ];

[0020] Step S4: Collision Detection: q near to q new Collision detection is performed on the generated path;

[0021] Step S5: Redundant node removal strategy: Use the redundant node removal strategy to replan the obtained path;

[0022] Step S6: Optimize the motion trajectory of the robotic arm.

[0023] Step S3 includes the following steps;

[0024] Step S3-1: If the random sampling point q rand Falling into the obstacle space Ω obs If the sample is within the range, discard this point and sample randomly again.

[0025] Step S3-2: If q rand If it falls within the non-obstacle space Ω, then it is determined by traversing the entire spanning tree node q. T =[x T y T , z T ], find q T With random sampling point q rand The node q with the closest Euclidean distance near =[x near y nｅar , z near ],Right now:

[0026]

[0027] Step S3-3: with To generate new nodes in a direction with a fixed step size, expressed by the formula:

[0028] q new =[x nｅw Y new , z nｅw Formula 2;

[0029] Step S3-4: If the random probability p of the sampling point is greater than P, the algorithm performs greedy sampling guided by an artificial potential field; then, based on the nodes q of the entire spanning tree... T =[x T y T , z T ], find q T With target node q goal =[x goal y goal , z goal The nearest node q in Euclidean distance. near =[x near y near , z near ],Right now:

[0030]

[0031] Step S3-5: Calculate node q near The net potential energy U it receives:

[0032] U = U att +U rep Formula 4;

[0033] In the formula, the total gravitational force acting on the robotic arm is expressed as:

[0034]

[0035]

[0036] U rep The total repulsive force experienced by the robotic arm:

[0037]

[0038]

[0039] Among them, U att (i) represents the gravitational force exerted on each link of the robotic arm by obstacles in space; U rep (i) represents the repulsive force exerted on each link of the robotic arm by obstacles in space; n represents the number of repulsive forces exerted on the links of the robotic arm by obstacles; d id represents the Euclidean distance between the actual position of the corresponding link joint of the robotic arm and the obstacle. o The range of the repulsive force field for each link; d s The safe distance for each link, k a k is the gravitational proportionality constant. r It is the repulsive force proportionality constant;

[0040] Step S3-6: After obtaining the net potential energy U, generate a new node q by expanding the vector U with a fixed step size step. new =[x new y new , z new If the potential field falls into a local minimum, the generation of the sampling point is discarded and random sampling is performed again.

[0041] Step S4 includes the following steps;

[0042] Step S4-1: Change path q near q new Divide the space into n points with equal step sizes. If any of these points falls within the obstacle space, it indicates a collision. If no collision occurs, q is incremented. new Store q in a random tree T. new Consider q near The parent node; if a collision is detected, discard q. new Return to step S3 and regenerate q new ;

[0043] Step S4-2: Repeat steps S3 to S4 until a new node q is generated. new With q goal If the Euclidean distance between nodes is less than the threshold thr, it means that the path planning has found the target point; the planned path is found by backtracking the random tree T according to the parent-child relationship of each node.

[0044] Step S5 specifically involves:

[0045] The obtained path is replanned using a redundant node removal strategy; the initial path nodes are stored in an array nums, nums = [q start ,q2,q3,…,q goal ], based on nums, starting from the initial point q start to q goal Perform path replanning, which means calculating the root node q in the array. root To the next node q next European distance:

[0046] d = ||q root -q next Formula 8;

[0047] If its Euclidean distance d is less than or equal to the original path length l:

[0048] l = step * (nm) (Formula Nine)

[0049] n represents the value of q in the array nums. next At this point, the index 'm' represents the value of 'q' in the array nums. root The index at this point. If its Euclidean distance is less than the original path length and the root node q root To the next node q next If the path does not collide with obstacles, then delete q. root With q next Redundant nodes between; and q next Updated to q next The next node; if the Euclidean distance d is greater than the original path length l or the root node q root To the next node q next If the path collides with an obstacle, then q root Updated to q next Repeat the above steps for the previous node until q next For q goal This indicates that the path replanning is complete.

[0050] Step S6 specifically involves:

[0051] After removing redundant nodes from the Cartesian coordinate system, the discrete nodes are solved using inverse kinematics to obtain the joint angles [θ1, θ2, θ3, θ4, θ5, θ6] of the robotic arm at each discrete point. The path nodes are then processed using polynomial interpolation, and the polynomial programming function for the robotic arm trajectory joint angles is as follows:

[0052] θ i (t)=a0+...+a i t i +...+a n t n Formula 10;

[0053] Find a0, ..., a in the function expression i ,…,a n The function can then be determined; differentiating this function with respect to time yields the expression for the joint angular velocity:

[0054]

[0055] Taking the derivative of the above equation with respect to time again, we obtain the expression for joint angular acceleration as follows:

[0056]

[0057] Assume the joint angle of the robotic arm is θ0 when it starts moving, and the angle of the joint is θ when it stops moving. f ;

[0058] Let the total motion time be t. f The initial joint angular velocity is The final joint angular velocity is The initial angular acceleration is The final joint angular acceleration is

[0059] Find the polynomial coefficients a0,…,a i ,…,a n Substituting the values ​​into the polynomial equation yields a smooth change in the joint angle of the robotic arm, thus completing the final robotic arm path planning.

[0060] The trajectory planning mentioned is obstacle avoidance path planning for robotic arms, which refers to the robotic arm planning a collision-free path within a limited time range when there are obstacles in the workspace.

[0061] The beneficial effects of this invention are as follows:

[0062] (1) This invention proposes a method based on non-obstacle space probabilistic potential field sampling, which ensures the probabilistic completeness of RRT sampling and uses artificial potential field-guided greedy sampling to give the algorithm a certain probability of target bias. This reduces collision detection and the generation of redundant sampling paths, and shortens the path generation time.

[0063] (2) A redundant node deletion strategy is proposed. By deleting redundant nodes, the path can be made shorter and more efficient.

[0064] (3) A polynomial interpolation trajectory optimization was proposed, which makes the generated final trajectory smoother and more suitable for the operation of the robotic arm. Attached Figure Description

[0065] The present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments:

[0066] Appendix Figure 1 This is a schematic diagram of the robotic arm described in this invention (taking the ABB-1200 robotic arm as an example, the table in the figure is the DH parameter table of the robotic arm described in this invention);

[0067] Appendix Figure 2 This is a schematic diagram of the overall workspace sampling as described in this invention;

[0068] Appendix Figure 3 This is a schematic diagram of the non-obstacle workspace sampling method described in this invention;

[0069] Appendix Figure 4 This is a schematic diagram of the trajectory after deleting redundant nodes as described in this invention;

[0070] Appendix Figure 5 This is a schematic diagram of the trajectory after processing with the fifth-order polynomial as described in this invention;

[0071] In the diagram: 1-connecting rod; 2-joint. Detailed Implementation

[0072] As shown in the figure, the RRT robotic arm trajectory planning method based on non-obstacle space probabilistic potential field sampling is used for a multi-link robotic arm with multiple joints 2. In the non-obstacle space, global path planning is performed by combining RRT with artificial potential field. The planning method includes generating sampling points based on the non-obstacle space, that is, filtering the sampling points by whether they fall into the obstacle space, avoiding the generation of redundant paths and unnecessary path collision detection.

[0073] The planning method also includes a probabilistic potential field greedy sampling strategy, which, under the premise of ensuring the completeness of the algorithm probability, gives the algorithm a target orientation with a preset probability to accelerate path generation, and reduces path collision detection and generates a relatively better path direction through the interaction of gravitational and repulsive fields.

[0074] The planning method also includes a redundant node removal strategy, which optimizes the path generated by the algorithm backtracking, removes redundant nodes in the path, and makes the generated path better; and smooths the optimized path by interpolation polynomial, so that the path is more in line with the actual movement of the robotic arm to reduce friction loss during violent movements.

[0075] The planning method includes the following steps;

[0076] Step S1: Mathematical modeling of the robotic arm: Describe the pose of the robotic arm and establish the link coordinate system of the robotic arm to obtain the DH model of the robotic arm;

[0077] Step S2: Initialize to perform global path planning, specifically: initialize tree T in the workspace, given an initial node q. start =[x start y start , z start ], target node q goal =[x goal y goal , z goal [ ], Extended fixed step size step, maximum number of iterations max, obstacle space Ω obs And the target point error allowable threshold thr;

[0078] Step S3: Execute the probabilistic potential field greedy sampling strategy, that is, use the non-obstacle space probabilistic potential field sampling to generate sampling points: Let the random probability threshold be P. When the generated random probability p is less than P, the algorithm performs random sampling to obtain random sampling points q.rand =[x rand y rand , z rand ];

[0079] Step S4: Collision Detection: q near to q new Collision detection is performed on the generated path;

[0080] Step S5: Redundant node removal strategy: Use the redundant node removal strategy to replan the obtained path;

[0081] Step S6: Optimize the motion trajectory of the robotic arm.

[0082] Step S3 includes the following steps;

[0083] Step S3-1: If the random sampling point q rand Falling into the obstacle space Ω obs If the sample is within the range, discard this point and sample randomly again.

[0084] Step S3-2: If q rand If it falls within the non-obstacle space Ω, then it is determined by traversing the entire spanning tree node q. T =[x T y T , z T ], find q T With random sampling point q rand The node q with the closest Euclidean distance near =[x near y near , z near ],Right now:

[0085]

[0086] Step S3-3: with To generate new nodes in a direction with a fixed step size, expressed by the formula:

[0087] q new =[x new y new , z new Formula 2;

[0088] Step S3-4: If the random probability p of the sampling point is greater than P, the algorithm performs greedy sampling guided by an artificial potential field; then, based on the nodes q of the entire spanning tree... T =[x T y T , z T ], find q T With target node q goal =[xgoal y goal , z goal The nearest node q in Euclidean distance. near =[x near y near , z near ],Right now:

[0089]

[0090] Step S3-5: Calculate node q near The net potential energy U it receives:

[0091] U = U att +U rep Formula 4;

[0092] In the formula, the total gravitational force acting on the robotic arm is expressed as:

[0093]

[0094]

[0095] U rep The total repulsive force experienced by the robotic arm:

[0096]

[0097]

[0098] Among them, U att (i) represents the gravitational force exerted on each link of the robotic arm by obstacles in space; U rep (i) represents the repulsive force exerted on each link of the robotic arm by obstacles in space; n represents the number of repulsive forces exerted on the links of the robotic arm by obstacles; d i d represents the Euclidean distance between the actual position of the corresponding link joint of the robotic arm and the obstacle. o The range of the repulsive force field for each link; d s The safe distance for each link, k a k is the gravitational proportionality constant. r It is the repulsive force proportionality constant;

[0099] Step S3-6: After obtaining the net potential energy U, generate a new node q by expanding the vector U with a fixed step size step. new =[x new y new , z new If the potential field falls into a local minimum, the generation of the sampling point is discarded and random sampling is performed again.

[0100] Step S4 includes the following steps;

[0101] Step S4-1: Change path q near q new Divide the space into n points with equal step sizes. If any of these points falls within the obstacle space, it indicates a collision. If no collision occurs, q is incremented. new Store q in a random tree T. new Consider q near The parent node; if a collision is detected, discard q. new Return to step S3 and regenerate q new ;

[0102] Step S4-2: Repeat steps S3 to S4 until a new node q is generated. new With q goal If the Euclidean distance between nodes is less than the threshold thr, it means that the path planning has found the target point; the planned path is found by backtracking the random tree T according to the parent-child relationship of each node.

[0103] Step S5 specifically involves:

[0104] The obtained path is replanned using a redundant node removal strategy; the initial path nodes are stored in an array nums, nums = [q start ,q2,q3,…,q goal ], based on nums, starting from the initial point q start to q goal Perform path replanning, which means calculating the root node q in the array. root To the next node q next European distance:

[0105] d = ||q root -q next Formula 8;

[0106] If its Euclidean distance d is less than or equal to the original path length l:

[0107] l = step * (nm) (Formula Nine)

[0108] n represents the value of q in the array nums. next At this point, the index 'm' represents the value of 'q' in the array nums. root The index at this point. If its Euclidean distance is less than the original path length and the root node q root To the next node q next If the path does not collide with obstacles, then delete q. root With q next Redundant nodes between; and q next Updated to q next The next node; if the Euclidean distance d is greater than the original path length l or the root node qroot To the next node q next If the path collides with an obstacle, then q root Updated to q next Repeat the above steps for the previous node until q next For q goal This indicates that the path replanning is complete.

[0109] Step S6 specifically involves:

[0110] After removing redundant nodes from the Cartesian coordinate system, the discrete nodes are solved using inverse kinematics to obtain the joint angles [θ1, θ2, θ3, θ4, θ5, θ6] of the robotic arm at each discrete point. The path nodes are then processed using polynomial interpolation, and the polynomial programming function for the robotic arm trajectory joint angles is as follows:

[0111] θ i (t)=a0+...+a i t i +...+a n t n Formula 10;

[0112] Find a0, ..., a in the function expression i ,…,a n The function can then be determined; differentiating this function with respect to time yields the expression for the joint angular velocity:

[0113]

[0114] Taking the derivative of the above equation with respect to time again, we obtain the expression for joint angular acceleration as follows:

[0115]

[0116] Assume the joint angle of the robotic arm is θ0 when it starts moving, and the angle of the joint is θ when it stops moving. f ;

[0117] Let the total motion time be t. f The initial joint angular velocity is The final joint angular velocity is The initial angular acceleration is The final joint angular acceleration is Find the polynomial coefficients a0,…,a i ,…,a n Substituting the values ​​into the polynomial equation yields a smooth change in the joint angle of the robotic arm, thus completing the final robotic arm path planning.

[0118] The trajectory planning mentioned is obstacle avoidance path planning for robotic arms, which refers to the robotic arm planning a collision-free path within a limited time range when there are obstacles in the workspace.

[0119] Example:

[0120] In this example, step S1 is for the ABB-1200 robotic arm, such as... Figure 1 As shown, the pose of the robot arm is described by the DH method and the link coordinate system of the robot arm is established, resulting in the DH model of the robot arm, as shown in the table in the figure.

[0121] In this example, during collision detection in step S4, q near to q new The generated path undergoes collision detection. Collision detection involves matching the path q... near q new Divide the space into 100 points with equal step sizes. If any of these points falls within the obstacle space, a collision occurs. If no collision occurs, q is incremented. new Store q in a random tree T. new Consider q near The parent node; if a collision is detected, discard q. new Return to step S3 and regenerate q new Repeat steps S3 to S4 until a new node q is generated. new With q goal If the Euclidean distance between nodes is less than the threshold thr, it means that the path planning has found the target point. The planned path is then found by backtracking through the random tree T based on the parent-child relationships between nodes.

[0122] In this example, the redundant node removal strategy in step S5 involves replanning the obtained path using the redundant node removal strategy. The initial path nodes are stored in an array nums, where nums = [q...]. start ,q2,q3,…,q goal ], based on nums, starting from the initial point q start to q goal Perform path replanning, which means calculating the root node q in the array. root To the next node q next Euclidean distance, if its Euclidean distance is less than the original path length and the root node q root To the next node q next If the path does not collide with obstacles, then delete q. root With q next Redundant nodes between them. The specific process involves initializing q... root For q start ,q next Let q2, if q root With q next If the distance between them is less than or equal to the original path length and no collision occurs, then update q. next For q3, delete q. rootThe nodes between q2 and q2, if q root With q next If the distance between them is greater than the original path length or a collision occurs, update q. root For q2, repeat the above steps until q next For q goal This indicates that the path replanning is complete.

[0123] In this example, during the polynomial trajectory optimization in step S6, the discrete nodes after deleting redundant nodes in the Cartesian space coordinates are solved using inverse kinematics to obtain the joint angles [θ1, θ2, θ3, θ4, θ5, θ6] of the robotic arm at each discrete point. Polynomial interpolation is then used to process the path nodes. The polynomial programming function for the joint angles of the robotic arm trajectory is as follows:

[0124] θ i (t)=a0+...+a i t i +...+a n t n

[0125] Find a0, ..., a in the function expression i ,…,a n The function can then be determined. Differentiating this function with respect to time yields the expression for the joint angular velocity:

[0126]

[0127] Taking the derivative of the above equation with respect to time again, we obtain the expression for joint angular acceleration as follows:

[0128]

[0129] Assume the joint angle of the robotic arm is θ0 when it starts moving, and the angle of the joint is θ when it stops moving. f Let the total motion time be t. f The initial joint angular velocity is The final joint angular velocity is The initial angular acceleration is The final joint angular acceleration is The polynomial coefficients a0,…,a can then be obtained. i ,…,a n Substituting these values ​​into the polynomial equation yields a smooth change in the joint angle of the robotic arm, thus completing the final robotic arm path planning.

Claims

1. A trajectory planning method for a robotic arm based on probabilistic potential field sampling in non-obstacle space using RRT (Reproducible Tracking Theorem) technology, applicable to multi-joint robotic arms, utilizes a combination of RRT and artificial potential field for global path planning in non-obstacle space. Its features include: The planning method includes generating sampling points based on non-obstacle space, that is, filtering sampling points by whether they fall into obstacle space, avoiding the generation of redundant paths and unnecessary path collision detection. The planning method also includes a probabilistic potential field greedy sampling strategy, which, under the premise of ensuring the completeness of the algorithm probability, gives the algorithm a target orientation with a preset probability to accelerate path generation, and reduces path collision detection and generates a relatively better path direction through the interaction of gravitational and repulsive fields. The planning method also includes a redundant node removal strategy, which optimizes the path generated by the algorithm backtracking, removes redundant nodes in the path, and makes the generated path better; and smooths the optimized path through interpolation polynomials to make the path more consistent with the actual movement of the robotic arm. The planning method includes the following steps; Step S1: Mathematical modeling of the robotic arm: Describe the pose of the robotic arm and establish the link coordinate system of the robotic arm to obtain the DH model of the robotic arm; Step S2: Initialize to perform global path planning, specifically: initialize tree T in the workspace, given an initial node q. start =[x start y start , z start ], target node q goal =[x goal y goal , z goal [ ], Extended fixed step size step, maximum number of iterations max, obstacle space Ω obs And the target point error allowable threshold thr; Step S3: Execute the probabilistic potential field greedy sampling strategy, that is, use the non-obstacle space probabilistic potential field sampling to generate sampling points: Let the random probability threshold be P. When the generated random probability p is less than P, the algorithm performs random sampling to obtain random sampling points q. rand =[x rand y rand , z rand ]; Step S4: Collision Detection: q near to q new Collision detection is performed on the generated path; Step S5: Redundant node removal strategy: Use the redundant node removal strategy to replan the obtained path; Step S6: Optimize the motion trajectory of the robotic arm; Step S3 includes the following steps; Step S3-1: If the random sampling point q rand Falling into the obstacle space Ω obs If the sample is within the range, discard this point and sample randomly again. Step S3-2: If q rand If it falls within the non-obstacle space Ω, then it is determined by traversing the entire spanning tree node q. T =[x T y T , z T ], find q T With random sampling point q rand The node q with the closest Euclidean distance near =[x near y near , z near ],Right now: Step S3-3: with To generate new nodes in a direction with a fixed step size, expressed by the formula: q new =[x new y new , z new Formula 2; Step S3-4: If the random probability p of the sampling point is greater than P, the algorithm performs greedy sampling guided by an artificial potential field; then, based on the nodes q of the entire spanning tree... T =[x T y T , z T ], find q T With target node q goal =[x goal y goal , z goal The nearest node q in Euclidean distance. near =[x near y near , z near ],Right now: Step S3-5: Calculate node q near The net potential energy U it receives: U=U att +U rep Official 4; In the formula, the total gravitational force acting on the robotic arm is expressed as: U rep The total repulsive force experienced by the robotic arm: Among them, U att (i) represents the gravitational force exerted on each link of the robotic arm by obstacles in space; U rep (i) represents the repulsive force exerted on each link of the robotic arm by obstacles in space; n represents the number of repulsive forces exerted on the links of the robotic arm by obstacles; d i d represents the Euclidean distance between the actual position of the corresponding link joint of the robotic arm and the obstacle. o The range of the repulsive force field for each link; d s The safe distance for each link, k a k is the gravitational proportionality constant. r It is the repulsive force proportionality constant; Step S3-6: After obtaining the net potential energy U, generate a new node q by expanding the vector U with a fixed step size step. new =[x new y new , z new If the potential field falls into a local minimum, the generation of the sampling point is discarded and random sampling is performed again.

2. The RRT robotic arm trajectory planning method based on non-obstacle space probability potential field sampling according to claim 1, characterized in that: Step S4 includes the following steps; Step S4-1: Change path q near q new Divide the space into n points with equal step sizes. If any of these points falls within the obstacle space, it indicates a collision. If no collision occurs, q is incremented. new Store q in a random tree T. new Consider q near The parent node; if a collision is detected, discard q. new Return to step S3 and regenerate q new ; Step S4-2: Repeat steps S3 to S4 until a new node q is generated. new With q goal If the Euclidean distance between nodes is less than the threshold thr, it means that the path planning has found the target point; the planned path is found by backtracking the random tree T according to the parent-child relationship of each node.

3. The RRT robotic arm trajectory planning method based on non-obstacle space probability potential field sampling according to claim 2, characterized in that: Step S5 specifically involves: The obtained path is replanned using a redundant node removal strategy; the initial path nodes are stored in an array nums, nums = [q start ,q2,q3,…,q goal ], based on nums, starting from the initial point q start to q goal Perform path replanning, which means calculating the root node q in the array. root To the next node q next European distance: d=||q root -q next Formula 8; If its Euclidean distance d is less than or equal to the original path length l: l = step * (nm) (Formula Nine) n represents the value of q in the array nums. next At this point, the index 'm' represents the value of 'q' in the array nums. root The index at this point; if its Euclidean distance is less than the original path length and the root node q root To the next node q next If the path does not collide with obstacles, then delete q. root With q next Redundant nodes between; and q next Updated to q next The next node; If the Euclidean distance d is greater than the original path length l or the root node q root To the next node q next If the path collides with an obstacle, then q root Updated to q next Repeat the above steps for the previous node until q next For q goal This indicates that the path replanning is complete.

4. The RRT robotic arm trajectory planning method based on non-obstacle space probability potential field sampling according to claim 3, characterized in that: Step S6 specifically involves: After removing redundant nodes from the Cartesian coordinate system, the discrete nodes are solved using inverse kinematics to obtain the joint angles [θ1, θ2, θ3, θ4, θ5, θ6] of the robotic arm at each discrete point. The path nodes are then processed using polynomial interpolation, and the polynomial programming function for the robotic arm trajectory joint angles is as follows: θ i (t)=a0+...+a i t i +...+a n t n Formula 10; Find a0, ..., a in the function expression i ,…,a n The function can then be determined; differentiating this function with respect to time yields the expression for the joint angular velocity: Taking the derivative of the above equation with respect to time again, we obtain the expression for joint angular acceleration as follows: Assume the joint angle of the robotic arm is θ0 when it starts moving, and the angle of the joint is θ when it stops moving. f ; Let the total motion time be t. f The initial joint angular velocity is The final joint angular velocity is The initial angular acceleration is The final joint angular acceleration is Find the polynomial coefficients a0,…,a i ,…,a n Substituting the values ​​into the polynomial equation yields a smooth change in the joint angle of the robotic arm, thus completing the final robotic arm path planning.

5. The RRT robotic arm trajectory planning method based on non-obstacle space probability potential field sampling according to claim 1, characterized in that: The trajectory planning mentioned is obstacle avoidance path planning for robotic arms, which refers to the robotic arm planning a collision-free path within a limited time range when there are obstacles in the workspace.

Images