Mechanical arm path planning method based on improved artificial potential field method and improved RRT-Connect algorithm
By integrating the improved artificial potential field method with the RRT-Connect algorithm, and introducing multiple expansion trees and sampling bias strategies, redundant nodes are eliminated, enabling smooth and efficient path planning for the robotic arm in complex environments.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- KUNMING UNIV OF SCI & TECH
- Filing Date
- 2024-05-29
- Publication Date
- 2026-05-19
AI Technical Summary
The traditional RRT-Connect algorithm suffers from problems such as uneven paths, slow search speed, and numerous redundant points in complex environments.
An improved artificial potential field method is adopted and the improved RRT-Connect algorithm is integrated. By introducing the third, fourth and fifth extension trees and combining the sampling bias strategy and the artificial potential field algorithm, redundant nodes are eliminated and a smooth path is formed.
It improves the efficiency and quality of path planning, solves the problems of uneven paths and slow search time in complex environments, and ensures that the robotic arm finds the asymptotically optimal route in various environments.
Smart Images
Figure CN118386246B_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of path planning technology, and more specifically to a robotic arm path planning method based on the fusion of an improved artificial potential field method and an improved RRT-Connect algorithm. Background Technology
[0002] With the development of robotics technology, mobile robots have begun to be widely used in various fields, and path planning is a key research direction in the field of robotics. Robotic arm path planning refers to planning a collision-free route from the starting point to the target point based on the robotic arm's structural parameters, environmental information factors, and the determination of the robotic arm's initial and target points. This route ensures that the robotic arm and robot do not experience excessively large turning angles during movement, thus achieving autonomous and collision-free operation.
[0003] Currently, there are three popular methods for path planning in robotic arms: sampling-based path planning, search-based path planning, and intelligent algorithm-based path planning. Sampling-based path planning algorithms include Rapidly-exploring Random Trees (RRT) and Probabilistic RoadMap (PRM). The main idea is to randomly sample within the robotic arm's workspace and optimize and filter the sampled points until a collision-free, optimal, or asymptotically optimal route is found. Search-based path planning methods include the A* algorithm and Dijkstra's algorithm. Their main idea is to search in the state space to find the optimal path from the starting point to the target point. Intelligent algorithm-based path planning methods include genetic algorithms and ant colony algorithms. Their main idea is to find the optimal path from the starting point to the target point by searching in the state space.
[0004] RRT is also a common path planning algorithm for robotic arms. However, the generation of random nodes in the RRT algorithm is not goal-oriented, which leads to a large number of redundant points during the sampling process. Furthermore, if the target point is in a complex environment, the RRT algorithm has the drawbacks of not being able to find a path, having too many turns, and having an uneven path.
[0005] To address this, a bidirectional RRT algorithm, namely the RRT-Connect algorithm, was developed for robotic arm path planning. However, the traditional RRT-Connect algorithm also suffers from problems such as uneven paths, slow search speed, and numerous redundant points in complex environments. Summary of the Invention
[0006] The purpose of this invention is to address the shortcomings of the prior art by proposing a robotic arm path planning method based on the fusion of an improved artificial potential field method and an improved RRT-Connect algorithm. This method provides a high-quality, smooth, stable, and time-efficient path planning method, solving the problems of uneven paths, slow search speed, and numerous redundant points in the traditional RRT-Connect algorithm under complex environments.
[0007] To achieve the above objectives, the present invention adopts the following technical solution:
[0008] The robotic arm path planning method based on the fusion of the improved artificial potential field method and the improved RRT-Connect algorithm specifically includes the following steps:
[0009] Step 1: System initialization. System initialization includes initializing the map information of the robotic arm's movement path to determine the size of obstacles, the coordinates of obstacle positions, the starting point of the robotic arm's path, and the ending point of the robotic arm's path.
[0010] Step 2: Based on the RRT-Connect algorithm, the third, fourth and fifth extension trees are introduced sequentially on the line connecting the starting point and the ending point of the robotic arm path. The coordinates of the root nodes and the sampling range of the third, fourth and fifth extension trees are determined. The sampling is expanded using the sampling range of the third, fourth and fifth extension trees as the domain to obtain the sampling points.
[0011] Step 3: Integrate the artificial potential field algorithm into the sampling points obtained by the RRT-Connect algorithm to obtain the final sampling points. Then, based on the principle that the sum of two sides is greater than the third side, eliminate the redundant nodes in the final sampling points to obtain a smooth path.
[0012] Step 4: Transmit the path point position information in the smooth path to the robot. The robot uses the position information to solve the inverse problem to obtain the angles of each joint and complete the planning operation under this path.
[0013] Furthermore, the system initialization in Step 1 also includes initializing the step size for each iteration, the distance threshold from the endpoint, the maximum number of iterations, the random sampling probability P, and the attraction and repulsion coefficients.
[0014] Furthermore, Step 2 specifically includes the following steps: Step 2.1: Based on the RRT-Connect algorithm, the third extension tree, the fourth extension tree, and the fifth extension tree are introduced sequentially on the line connecting the start point and the end point of the robotic arm path to determine whether there are obstacles on the line connecting the start point and the end point of the robotic arm path.
[0015] Step 2.2: If there are no obstacles on the line connecting the starting point and the ending point of the robotic arm path, then select one-quarter of the line connecting the starting point and the ending point of the robotic arm path as the root node coordinates of the third extension tree, two-quarters as the root node coordinates of the fourth extension tree, and three-quarters as the root node coordinates of the fifth extension tree.
[0016] Step 2.3: If there are obstacles on the line connecting the starting point and the ending point of the robotic arm path, the coordinates of the root nodes of the third, fourth, and fifth extended trees are obtained based on the bias strategy.
[0017] Step 2.4: Based on the root node coordinates of the third, fourth, and fifth extended trees obtained in Step 2.2 or Step 2.3, determine the sampling range of the starting tree and the third extended tree, the sampling range of the ending tree and the fifth extended tree, and the sampling range of the fourth extended tree.
[0018] Step 2.5: Based on the sampling range of the starting tree and the third extended tree, the sampling range of the ending tree and the fifth extended tree, and the sampling range of the fourth extended tree, sampling begins sequentially from the starting tree, the third tree, the fourth tree, the fifth tree, and the ending tree.
[0019] Furthermore, Step 2.4 determines the sampling range of the starting tree and the third extended tree, the sampling range of the ending tree and the fifth extended tree, and the sampling range of the fourth extended tree, specifically as follows:
[0020] The sampling range of the starting tree and the third extended tree is determined as follows: the center of the line connecting the starting tree and the third extended tree is taken as the center, and the radius is 1.5 times the distance from the starting point to the center of the circle.
[0021] The sampling range of the endpoint tree and the fifth expansion tree is determined as follows: the midpoint of the line connecting the endpoint tree and the fifth expansion tree is taken as the center, and the radius is 1.5 times the distance from the endpoint to the center of the circle.
[0022] The sampling range of the fourth expansion tree is determined as follows: with the fourth expansion tree as the center, the radius is 1.5 times the distance from the fourth expansion tree to the fifth expansion tree, which is used as the sampling range of the fourth expansion tree.
[0023] Furthermore, in Step 2.5, sampling begins from the starting tree, the third tree, the fourth tree, the fifth tree, and the ending tree; specifically, it includes the following steps: Step 2.51, First, the starting tree expands towards the third expansion tree with a certain probability, and the third expansion tree expands directly towards the starting tree;
[0024] If no obstacle is detected during the expansion of the starting tree and the third extended tree, the third extended tree will continue to expand toward the starting tree until the third extended tree intersects with the starting tree.
[0025] If an obstacle is detected during the expansion of the starting tree and the third expansion tree, the third expansion tree expands toward the starting tree with a certain probability, and the starting tree expands directly toward the third expansion tree.
[0026] Step 2.52: Next, the fourth expansion tree expands towards the third expansion tree and the fifth expansion tree with a certain probability.
[0027] Step 2.53: Then, the fifth expansion tree expands towards the terminal tree with a certain probability, and the terminal tree expands directly towards the fifth expansion tree;
[0028] If an obstacle is detected during the expansion of the fifth extension tree and the endpoint tree, the endpoint tree will continue to expand toward the fifth extension tree until the fifth extension tree and the endpoint tree intersect.
[0029] If no obstacle is detected during the expansion of the fifth expansion tree and the endpoint tree, the endpoint tree expands toward the fifth expansion tree with a probability of one point, and the fifth expansion tree expands directly toward the endpoint tree.
[0030] Step 2.54. Finally, repeat the above operations until the starting tree intersects with the third extended tree, the third extended tree intersects with the fourth extended tree, the fourth extended tree intersects with the fifth extended tree, and the fifth extended tree intersects with the ending tree.
[0031] If the starting tree intersects with the third extended tree, but the fourth extended tree does not intersect with the third extended tree, then the third extended tree also begins to expand towards intersecting with the fifth extended tree, and the starting tree stops expanding.
[0032] If the endpoint tree intersects with the fifth tree, but the fourth extended tree does not intersect with the third extended tree, then the third extended tree also begins to expand toward the fourth extended tree and intersects with it, and the starting tree stops expanding.
[0033] Furthermore, in the cases described in Steps 2.51 to 2.53, i.e., when the starting tree and the third extended tree do not intersect, and the ending tree and the fifth extended tree do not intersect, the method for selecting the extended sampling points can be calculated using the following formula:
[0034]
[0035] In the formula, p1 is the probability that the sampling point is a specified point, which is set to 0.2 here.
[0036] Furthermore, in Step 2.54, if the starting tree intersects with the third extended tree, but the fourth extended tree does not intersect with the third extended tree, then the third extended tree also begins to expand towards and intersect with the fifth extended tree, and the starting tree stops expanding; if the ending tree intersects with the fifth tree, but the fourth extended tree does not intersect with the third extended tree, then the third extended tree also begins to expand towards and intersect with the fourth extended tree, and the starting tree stops expanding.
[0037] In both of the above cases—that is, when the starting tree intersects with the third extended tree and the ending tree intersects with the fifth extended tree—the selection method for the extended sampling points can be calculated using the following formula:
[0038]
[0039] Wherein, p1 is the probability that the sampling point is the specified point, which is set to 0.2 here, and p2 is the maximum value of the interval from which the fourth tree takes the second specified point, which is set to 0.4 here;
[0040] Furthermore, Step 3 specifically includes the following steps: Step 3.1, fusing the artificial potential field algorithm into the sampling points obtained by the RRT-Connect algorithm, and using the gravitational and repulsive properties in the artificial potential field to calculate the gravitational force F experienced by the nearest point. grav and the repulsive force F received rep ;
[0041] F grav =ερ(q) goal -q)
[0042] In the formula, ε is the gravitational constant, which is set to 0.000001 here, and ρ(q goal -q) represents the distance between the random point and the target point.
[0043]
[0044] In the formula, ε is the gravitational constant, and ρ(q) goal -q) represents the distance between the random point and the target point; σ is the repulsive constant, ρ(q,q) obj ) represents the distance between the random point and the obstacle, and ρ0 represents the distance affected by the obstacle;
[0045] Step 3.2, take the repulsive force F from Step 3.1 rep With repulsive force F rep The resultant force is used as a new extended node, and the selection of the new extended node can be obtained according to the following formula;
[0046]
[0047] Where, when ρ(q,q) objWhen ) < ρ0, X new The repulsive force needs to be subtracted, where s represents the iteration step size;
[0048] Step 3.3: Connect the newly expanded nodes selected in Step 3.2 sequentially to form a path. Then, using the formula that the sum of any two sides is greater than the third side, eliminate redundant nodes in the newly expanded nodes of the path to obtain a smooth path.
[0049] a+b>c
[0050] In the formula, a, b, and c are any side of the triangle.
[0051] Compared with the prior art, one of the following beneficial effects of the present invention is:
[0052] 1. This invention improves the efficiency and quality of path planning by integrating the improved artificial potential field method with the improved RRT-Connect algorithm, enabling the robotic arm to find an asymptotically optimal route when dealing with various environments.
[0053] 2. This invention solves the problems of slow search time and long path in complex environments by adding third, fourth and fifth extension trees to the original RRT-Connect algorithm, specifying the sampling range of each extension tree, and introducing a sampling bias strategy.
[0054] 3. This invention introduces the principle that the sum of two sides of a triangle is greater than the third side by using the artificial potential field method, thereby eliminating redundant sampling nodes and solving the problem of uneven paths and excessive turning points that lead to excessive joint transitions in the robotic arm. Attached Figure Description
[0055] Figure 1 This is a schematic diagram of the overall process of the robotic arm path planning method based on the fusion of the improved artificial potential field method and the improved RRT-Connect algorithm of the present invention.
[0056] Figure 2 Flowchart of the improved RRT-Connect algorithm;
[0057] Figure 3 Flowchart for the improved artificial potential field method;
[0058] Figure 4 Schematic diagram of the improved RRT-Connect algorithm;
[0059] Figure 5 A schematic diagram of the improved artificial potential field method;
[0060] Figure 6 This is the initial, unoptimized two-dimensional obstacle avoidance path diagram;
[0061] Figure 7 This is the improved two-dimensional obstacle avoidance path diagram. Detailed Implementation
[0062] The present invention will be further described below with reference to the accompanying drawings and specific embodiments:
[0063] The present invention provides a robotic arm path planning method based on the fusion of an improved artificial potential field method and an improved RRT-Connect algorithm, such as... Figure 1 As shown, it includes the following steps:
[0064] Step 1: Initialize the system. System initialization includes initializing the map information of the robotic arm's movement path to determine the size of obstacles, the coordinates of obstacle positions, the starting point of the robotic arm's path, and the ending point of the robotic arm's path.
[0065] The system initialization also includes initializing the step size for each iteration, the distance threshold from the endpoint, the maximum number of iterations, the random sampling probability P, and the attraction and repulsion coefficients.
[0066] It should be noted that the step size for each iteration is set to 10, the distance to the endpoint threshold is set to 5, and the maximum number of iterations is set to 10,000.
[0067] Step 2: Based on the RRT-Connect algorithm, a third, fourth, and fifth extension tree are sequentially introduced along the line connecting the start and end points of the robotic arm path. The coordinates of the root nodes and the sampling range of the third, fourth, and fifth extension trees are determined. Extended sampling is performed using the sampling range of the third, fourth, and fifth extension trees as the neighborhood to obtain sampling points, such as... Figure 2 and Figure 4 Specifically, it includes the following steps:
[0068] Step 2.1: Based on the RRT-Connect algorithm, the third extension tree, the fourth extension tree, and the fifth extension tree are introduced sequentially on the line connecting the start point and the end point of the robotic arm path to determine whether there are obstacles on the line connecting the start point and the end point of the robotic arm path.
[0069] Step 2.2: If there are no obstacles on the line connecting the starting point and the ending point of the robotic arm path, then select one-quarter of the line connecting the starting point and the ending point of the robotic arm path as the root node coordinates of the third extension tree, two-quarters as the root node coordinates of the fourth extension tree, and three-quarters as the root node coordinates of the fifth extension tree.
[0070] Step 2.3: If there are obstacles on the line connecting the starting point and the ending point of the robotic arm path, the coordinates of the root nodes of the third, fourth, and fifth extended trees are obtained based on the bias strategy.
[0071] Step 2.4: Based on the root node coordinates of the third, fourth, and fifth extended trees obtained in Step 2.2 or Step 2.3, determine the sampling range of the starting tree and the third extended tree, the sampling range of the ending tree and the fifth extended tree, and the sampling range of the fourth extended tree. Specifically:
[0072] The sampling range of the starting tree and the third extended tree is determined as follows: the center of the line connecting the starting tree and the third extended tree is taken as the center, and the radius is 1.5 times the distance from the starting point to the center of the circle.
[0073] The sampling range of the endpoint tree and the fifth expansion tree is determined as follows: the midpoint of the line connecting the endpoint tree and the fifth expansion tree is taken as the center, and the radius is 1.5 times the distance from the endpoint to the center of the circle.
[0074] The sampling range of the fourth expansion tree is determined as follows: with the fourth expansion tree as the center, the radius is 1.5 times the distance from the fourth expansion tree to the fifth expansion tree, which is used as the sampling range of the fourth expansion tree.
[0075] Step 2.5: Based on the sampling ranges of the starting tree and the third extended tree, the sampling ranges of the ending tree and the fifth extended tree, and the sampling range of the fourth extended tree, sampling is performed sequentially from the starting tree, the third tree, the fourth tree, the fifth tree, and the ending tree. This includes the following steps:
[0076] Step 2.51: First, the starting tree expands towards the third expansion tree with a certain probability, and the third expansion tree expands directly towards the starting tree.
[0077] If no obstacle is detected during the expansion of the starting tree and the third extended tree, the third extended tree will continue to expand toward the starting tree until the third extended tree intersects with the starting tree.
[0078] If an obstacle is detected during the expansion of the starting tree and the third expansion tree, the third expansion tree expands toward the starting tree with a certain probability, and the starting tree expands directly toward the third expansion tree.
[0079] Step 2.52: Next, the fourth expansion tree expands towards the third expansion tree and the fifth expansion tree with a certain probability.
[0080] Step 2.53: Then, the fifth expansion tree expands towards the terminal tree with a certain probability, and the terminal tree expands directly towards the fifth expansion tree;
[0081] If an obstacle is detected during the expansion of the fifth extension tree and the endpoint tree, the endpoint tree will continue to expand toward the fifth extension tree until the fifth extension tree and the endpoint tree intersect.
[0082] If no obstacle is detected during the expansion of the fifth extension tree and the endpoint tree, the endpoint tree expands toward the fifth extension tree with a probability of one point, and the fifth extension tree expands directly toward the endpoint tree.
[0083] It should be noted that in Steps 2.51 to 2.53, where the starting tree does not intersect with the third extended tree and the ending tree does not intersect with the fifth extended tree, the selection method for the extended sampling points can be calculated using the following formula:
[0084]
[0085] Wherein, p1 is the probability that the sampling point is a specified point, which is set to 0.2 here;
[0086] It should be noted that when the probability of random sampling is less than p1, the sampling point is a pre-set point; when the probability of random sampling is greater than p1, the sampling point is a random point.
[0087] Step 2.54. Finally, repeat the above operations until the starting tree intersects with the third extended tree, the third extended tree intersects with the fourth extended tree, the fourth extended tree intersects with the fifth extended tree, and the fifth extended tree intersects with the ending tree.
[0088] If the starting tree intersects with the third extended tree, but the fourth extended tree does not intersect with the third extended tree, then the third extended tree also begins to expand towards intersecting with the fifth extended tree, and the starting tree stops expanding.
[0089] If the endpoint tree intersects with the fifth tree, but the fourth extended tree does not intersect with the third extended tree, then the third extended tree also begins to expand toward the fourth extended tree and intersects with it, and the starting tree stops expanding.
[0090] It should be noted that in Step 2.54, if the starting tree intersects with the third extended tree, but the fourth extended tree does not intersect with the third extended tree, then the third extended tree also begins to expand towards intersecting with the fifth extended tree, and the starting tree stops expanding.
[0091] If the endpoint tree intersects with the fifth tree, but the fourth expansion tree does not intersect with the third expansion tree, then the third expansion tree also begins to expand toward the fourth expansion tree and intersects with it, and the starting tree stops expanding.
[0092] In both of the above cases—that is, when the starting tree intersects with the third extended tree and the ending tree intersects with the fifth extended tree—the selection method for the extended sampling points can be calculated using the following formula:
[0093]
[0094] Wherein, p1 is the probability that the sampling point is the specified point, which is set to 0.2 here, and p2 is the maximum value of the interval from which the fourth tree takes the second specified point, which is set to 0.4 here;
[0095] Step 3: To make the sampling points more ideal and improve algorithm efficiency, an artificial potential field algorithm is fused into the sampling points obtained by the RRT-Connect algorithm to obtain the final sampling points. Then, based on the principle that the sum of two sides is greater than the third side, redundant nodes in the final sampling points are eliminated to obtain a smooth path, such as... Figure 3-7 As shown, the specific steps include the following:
[0096] Step 3.1: Integrate the artificial potential field algorithm into the sampling points obtained by the RRT-Connect algorithm, and use the gravitational and repulsive properties in the artificial potential field to calculate the gravitational force F on the nearest point. grav and the repulsive force F received rep ;
[0097] F grav =ερ(q) goal -q)
[0098] In the formula, ε is the gravitational constant, which is set to 0.000001 here, and ρ(q goal -q) represents the distance between the random point and the target point.
[0099]
[0100] In the formula, ε is the gravitational constant, and ρ(q) goal -q) represents the distance between the random point and the target point; σ is the repulsive constant, ρ(q,q) obj ) represents the distance between the random point and the obstacle, and ρ0 represents the distance affected by the obstacle;
[0101] It should be noted that the closer a random point is to an obstacle, the stronger the repulsive force becomes; when the point is more than a certain distance from the obstacle, the repulsive force becomes zero. Conversely, the farther a random point is from the target point, the stronger the attractive force becomes, and vice versa.
[0102] Step 3.2, take the repulsive force F from Step 3.1 rep With repulsive force F rep The resultant force is used as a new extended node, and the selection of the new extended node can be obtained according to the following formula;
[0103]
[0104] Where, when ρ(q,q) obj When ) < ρ0, X new The repulsive force needs to be subtracted, where s represents the iteration step size;
[0105] Step 3.3: Connect the newly expanded nodes selected in Step 3.2 sequentially to form a path. Then, using the formula that the sum of any two sides is greater than the third side, eliminate redundant nodes in the newly expanded nodes of the path to obtain a smooth path.
[0106] a+b>c
[0107] In the formula, a, b, and c are any side of the triangle;
[0108] It should be noted that by utilizing the principle that the sum of two sides of a triangle is greater than the third side, redundant nodes are eliminated, making the path of the robotic arm smoother and preventing large angle changes in each joint.
[0109] Step 4: Transmit the path point position information in the smooth path to the robot. The robot uses the position information to solve the inverse problem to obtain the angles of each joint and complete the planning operation under this path.
[0110] The specific embodiments described above further illustrate the purpose, technical solution, and beneficial effects of the present invention, and are merely preferred embodiments of the present invention, not intended to limit the present invention in any other way. Therefore, those skilled in the art can make various modifications or variations based on the technical teachings disclosed in the present invention, as long as the modifications or variations do not depart from the essence of the present invention, they are still within the protection scope of the present invention.
Claims
1. A robotic arm path planning method based on the fusion of an improved artificial potential field method and an improved RRT-Connect algorithm, characterized in that: Specifically, the following steps are included: Step 1: System initialization. System initialization includes initializing the map information of the robotic arm's movement path to determine the size of obstacles, the coordinates of obstacle positions, the starting point of the robotic arm's path, and the ending point of the robotic arm's path. Step 2: Based on the RRT-Connect algorithm, the third, fourth and fifth extension trees are introduced sequentially on the line connecting the starting point and the ending point of the robotic arm path. The coordinates of the root nodes and the sampling range of the third, fourth and fifth extension trees are determined. The sampling is expanded using the sampling range of the third, fourth and fifth extension trees as the domain to obtain the sampling points. Step 3: Integrate the artificial potential field algorithm into the sampling points obtained by the RRT-Connect algorithm to obtain the final sampling points. Then, based on the principle that the sum of two sides is greater than the third side, eliminate the redundant nodes in the final sampling points to obtain a smooth path. Step 4: Transmit the path point position information in the smooth path to the robot. The robot uses the position information to solve the inverse solution to obtain the angle of each joint of the robot and complete the planning operation under the path. The system initialization in Step 1 also includes initializing the step size of each iteration, the distance to the endpoint threshold, the maximum number of iterations, the random sampling probability P, and the attraction and repulsion coefficients. Step 2 specifically includes the following steps: Step 2.1: Based on the RRT-Connect algorithm, the third extension tree, the fourth extension tree, and the fifth extension tree are introduced sequentially on the line connecting the start point and the end point of the robotic arm path to determine whether there are obstacles on the line connecting the start point and the end point of the robotic arm path. Step 2.2: If there are no obstacles on the line connecting the starting point and the ending point of the robotic arm path, then select one-quarter of the line connecting the starting point and the ending point of the robotic arm path as the root node coordinates of the third extension tree, two-quarters as the root node coordinates of the fourth extension tree, and three-quarters as the root node coordinates of the fifth extension tree. Step 2.3: If there are obstacles on the line connecting the starting point and the ending point of the robotic arm path, the coordinates of the root nodes of the third, fourth, and fifth extended trees are obtained based on the bias strategy. Step 2.4: Based on the root node coordinates of the third, fourth, and fifth extended trees obtained in Step 2.2 or Step 2.3, determine the sampling range of the starting tree and the third extended tree, the sampling range of the ending tree and the fifth extended tree, and the sampling range of the fourth extended tree. Step 2.5: Based on the sampling range of the starting tree and the third extended tree, the sampling range of the ending tree and the fifth extended tree, and the sampling range of the fourth extended tree, sampling begins sequentially from the starting tree, the third tree, the fourth tree, the fifth tree, and the ending tree. Step 2.5 involves sampling starting from the starting tree, the third tree, the fourth tree, the fifth tree, and the ending tree; specifically, it includes the following steps: Step 2.51: First, the starting tree expands towards the third expansion tree with a certain probability, and the third expansion tree expands directly towards the starting tree. If no obstacle is detected during the expansion of the starting tree and the third extended tree, the third extended tree will continue to expand toward the starting tree until the third extended tree intersects with the starting tree. If an obstacle is detected during the expansion of the starting tree and the third expansion tree, the third expansion tree expands toward the starting tree with a certain probability, and the starting tree expands directly toward the third expansion tree. Step 2.52: Next, the fourth expansion tree expands towards the third expansion tree and the fifth expansion tree with a certain probability. Step 2.53: Then, the fifth expansion tree expands towards the terminal tree with a certain probability, and the terminal tree expands directly towards the fifth expansion tree; If an obstacle is detected during the expansion of the fifth extension tree and the endpoint tree, the endpoint tree will continue to expand toward the fifth extension tree until the fifth extension tree and the endpoint tree intersect. If no obstacle is detected during the expansion of the fifth expansion tree and the endpoint tree, the endpoint tree expands toward the fifth expansion tree with a probability of one point, and the fifth expansion tree expands directly toward the endpoint tree. Step 2.
54. Finally, repeat the above operations until the starting tree intersects with the third extended tree, the third extended tree intersects with the fourth extended tree, the fourth extended tree intersects with the fifth extended tree, and the fifth extended tree intersects with the ending tree. If the starting tree intersects with the third extended tree, but the fourth extended tree does not intersect with the third extended tree, then the third extended tree also begins to expand towards intersecting with the fifth extended tree, and the starting tree stops expanding. If the endpoint tree intersects with the fifth tree, but the fourth extended tree does not intersect with the third extended tree, then the third extended tree also begins to expand toward the fourth extended tree and intersects with it, and the starting tree stops expanding. Step 3 specifically includes the following steps: Step 3.1: Integrate the artificial potential field algorithm into the sampling points obtained by the RRT-Connect algorithm, and use the gravitational and repulsive properties in the artificial potential field to calculate the gravitational force on the nearest point. and the repulsive force received ; ； Where, in the formula, The gravitational constant is set to 0.000001 here. This represents the distance between a random point and the target point; ； in, It is the repulsive constant. This represents the distance between a random point and an obstacle. Indicates the distance affected by obstacles; Step 3.2, take the repulsive force from Step 3.1 With gravity The resultant force is used as a new extended node, and the selection of the new extended node can be obtained according to the following formula; ； Among them, when hour, The repulsive force needs to be subtracted. Indicates the step size of each iteration. Step 3.3: Connect the newly expanded nodes selected in Step 3.2 sequentially to form a path. Then, using the formula that the sum of any two sides is greater than the third side, eliminate redundant nodes in the newly expanded nodes of the path to obtain a smooth path. ； In the formula, a, b, and c are any side of the triangle.
2. The robotic arm path planning method based on the fusion of the improved artificial potential field method and the improved RRT-Connect algorithm according to claim 1, characterized in that: In Step 2.4, determining the sampling range of the starting tree and the third extended tree, the sampling range of the ending tree and the fifth extended tree, and the sampling range of the fourth extended tree specifically involves: The sampling range of the starting tree and the third extended tree is determined as follows: the center of the line connecting the starting tree and the third extended tree is taken as the center, and the radius is 1.5 times the distance from the starting point to the center of the circle. The sampling range of the endpoint tree and the fifth expansion tree is determined as follows: the midpoint of the line connecting the endpoint tree and the fifth expansion tree is taken as the center, and the radius is 1.5 times the distance from the endpoint to the center of the circle. The sampling range of the fourth expansion tree is determined as follows: with the fourth expansion tree as the center, the radius is 1.5 times the distance from the fourth expansion tree to the fifth expansion tree, which is used as the sampling range of the fourth expansion tree.
3. The robotic arm path planning method based on the fusion of the improved artificial potential field method and the improved RRT-Connect algorithm according to claim 1, characterized in that: In the cases described in Steps 2.51 to 2.53, where the starting tree does not intersect with the third extended tree and the ending tree does not intersect with the fifth extended tree, the selection method for the extended sampling points can be calculated using the following formula: ； ； ； ； Where, in the formula, This represents the probability that the sampling point is a specified point, which is set to 0.2 here.
4. The robotic arm path planning method based on the fusion of the improved artificial potential field method and the improved RRT-Connect algorithm according to claim 1, characterized in that: In Step 2.54, if the starting tree intersects with the third extended tree, but the fourth extended tree does not intersect with the third extended tree, then the third extended tree also begins to expand towards and intersect with the fifth extended tree, and the starting tree stops expanding; if the ending tree intersects with the fifth tree, but the fourth extended tree does not intersect with the third extended tree, then the third extended tree also begins to expand towards and intersect with the fourth extended tree, and the starting tree stops expanding. In both of the above cases—that is, when the starting tree intersects with the third extended tree and the ending tree intersects with the fifth extended tree—the selection method for the extended sampling points can be calculated using the following formula: ； ； ； Where, in the formula This represents the probability that the sampling point is a specified point; here, it is set to 0.
2. The maximum value of the interval from the second specified point to the fourth tree is set to 0.4 here.