Obstacle avoidance path planning method for robotic arm based on RRT*FN algorithm

Abstract

The present invention provides a robot arm obstacle avoidance path planning method based on the RRT*FN algorithm, comprising the following steps: establishing a robot arm kinematic model; collision detection; initializing a workspace; using an improved RRT*FN algorithm applicable to multiple scenarios to plan a global path for the robot arm, wherein the random sampling point s rand Generation: Based on the nearest random tree node s nearest Based on the properties of the algorithm, new nodes are generated using a binary greedy expansion method, a safe expansion strategy, and a local environment sampling boundary expansion strategy. These new nodes are then further expanded toward the target point, a process known as secondary expansion. Feasibility screening is used to determine whether to add or discard these new nodes, while an ellipsoid is used to limit the total number of nodes in the search tree. A method based on triangle inequalities is used to determine and optimize the current path. This method meets the requirements for a real-time optimal path while being adaptable to a variety of scenarios.

Description

Technical Field

[0001] The present invention relates to the fields of robotics, computer vision, and artificial intelligence technology, and in particular to a method for obstacle avoidance path planning for a robotic arm based on an RRT*FN algorithm. Background Art

[0002] With technological advancements and growing demand, robotic arms are playing an increasingly important role in many fields, such as cargo handling and sorting in the logistics industry. As these fields expand, the environments surrounding robotic arms for home use or logistics are no longer simply fixed, simple environments like those found in industrial robotic arms; they are increasingly complex and diverse, placing higher demands on the ability of robotic arm path planning to adapt to these diverse environments. Currently, the RRT algorithm, a sampling-based planning method more suitable for high-dimensional path planning, is commonly used for high-dimensional path planning. The RRT algorithm aims to create a tree from a starting point and explore a collision-free path to the target point by randomly sampling and growing the tree from the configuration space. However, the high randomness of the RRT algorithm inevitably leads to long planning times and instability. The RRT*FN algorithm, based on the RRT* algorithm, limits the maximum number of nodes in the tree. When the maximum number of nodes is reached, the corresponding node is removed to ensure that the total number of nodes remains constant while new nodes are added during incremental sampling. While this method avoids unbounded tree growth and saves memory, it does not significantly improve planning speed and suffers from low convergence accuracy.

[0003] Furthermore, the complexity of the environment inevitably leads to problems with narrow channel path planning, and current research on this area of ​​path planning is still in the exploratory stage. Kuffner et al. proposed the RRT-Connect algorithm, which uses a dual-tree search state space to significantly improve search efficiency and also offers a solution for path planning in environments with narrow channels on one side. However, it does not provide an optimal solution, and in bilateral narrow channel environments, its algorithm is almost identical to the RRT single-branch tree. Wu Zhenping et al. proposed a randomized steering expansion strategy. This method randomly generates a direction for expansion after expansion guided by a random sampling point fails. If the edge formed by the random point and the new sampling point is unobstructed, it is added to the tree; otherwise, another random state is sampled. The randomized steering expansion strategy has a positive effect on narrow channel path planning, but it also suffers from significant randomness. Li BH et al. proposed a method to first determine the environment type and then select an appropriate direction for retrying expansion. This significantly improved path planning in environments with narrow channels. However, due to the diversity of environment types, the method proposed by Li BH et al. does not fully address the diverse environment types. There are still problems such as long path processing time in complex environments and failure to meet real-time requirements, and the inability to search for an effective path for the robotic arm in narrow channel environments. Summary of the Invention

[0004] The purpose of the present invention is to overcome the above shortcomings and propose a robot arm obstacle avoidance path planning method based on the RRT*FN algorithm that can meet the real-time optimal path conditions and adapt to various scenarios.

[0005] A method for obstacle avoidance path planning of a robotic arm based on an RRT*FN algorithm of the present invention comprises the following steps:

[0006] S1: Establishing a kinematic model: establishing a kinematic model of the robot arm, and performing kinematic analysis of the robot arm based on the established kinematic model;

[0007] The robotic arm satisfies the Pieper principle, and an analytical method is used to obtain an inverse kinematic solution for the robotic arm.

[0008] S2: Collision detection: The ball envelope method and the box envelope method are used to detect collisions between the robot arm and obstacles, as well as between the robot arm links.

[0009] S3: Initialize the workspace: Initialize the workspace where the robot moves and the environmental information in the workspace. The workspace is divided into a free area and an obstacle area, and the initial point s for the robot to move is given. init and the target point s to which the robot moves goal ;

[0010] S4: Use the improved RRT*FN algorithm applicable to multiple scenarios to plan the global path for the robot arm. The specific steps are as follows:

[0011] S41: Initialize the search tree and set the initial point s init Set as random tree root node T init Initialize the extended random tree region T_R and set the closest distance R between the node on the extended tree and the target point to s init to s goal The distance between them; set the total number of nodes;

[0012] S42: Generate random sampling points s rand , the process is as follows:

[0013] S421: Calibrate the entire sampling space to the interval [0,1], and set the probability value P goal_point , probability value P unexplored and probability value P T_R , divide the [0,1] interval into four sub-intervals, namely the target point interval [0,P goal_point ]、Unexplored interval[P goal_point ,P unexplored ], goal-oriented interval [P unexplored ,PT_R ]、The interval where the expanded tree is located [P T_R ,1];

[0014] S422: If the probability of random generation is in [0,P goal_point ], then the sample is uniformly collected within a sphere with a certain radius and the target point as the center; if the probability of random generation is between [P goal_point ,P unexplored ], then sample in the unexplored area; if the probability of random generation is between [P T_R ,1], then uniform sampling is performed in the area where the expanded tree is located; if none of the above is true, random sampling points are sampled from the target-oriented interval;

[0015] S43: traverse the random tree and search the random tree distance s rand nearest random tree node s nearest ;

[0016] S44: Determine the nearest random tree node s nearest Attributes: If s nearest Along the s nearest Point to s rand The position point obtained by walking distance Step_to_random in the direction is located within the obstacle and the boundary extension mark is 0, then judge s nearest is an unexpanded boundary point, and a new node is generated by adopting a boundary expansion strategy based on local environment sampling; if s nearest The boundary extension mark is 1, then judge s nearest For the extended boundary points, the extension of the new node is in the direction away from the obstacle, that is, the mean point of the obstacle area point set in the local sampling points to the nearest random tree node s nearest direction, the step size is set to Step_to_random; otherwise, a new node is generated by greedy expansion using binary search;

[0017] The method of generating a new node based on the local environment sampling boundary expansion strategy includes the following steps:

[0018] Step 1: To obtain enough sampling points that meet the requirements to describe the local structure, s nearest As the center, each dimension of its spatial coordinates is independently taken to have a positive and negative distance ±Step_to_random, forming s nearest Then, with the obtained sampling point as the center, each dimension of its spatial coordinate is independently taken as half of the positive and negative distance ±Step_to_random / 2 to form a new sampling point;

[0019] Step 2: Obtain the sampling point set S sampling_points Then these sampling points are divided into the free zone sampling point set Sloc_free and the obstacle area sampling point set S loc_obs , then calculate the mean point s of the obstacle area sampling point set avepoint ;

[0020] Step 3: If the mean point is within the obstacle, the two sampling points s that are farthest apart along the obstacle area point set are obs1 and s obs2 The line is extended in one direction or two directions to form a new node; if the mean point is within the obstacle and the one-way or two-way extension fails, the distance s in the free zone is taken nearest The farthest point is used as the new node;

[0021] The two points s that are farthest apart along the obstacle area point set obs1 and s obs2 Connection unidirectional or bidirectional extension: If s nearest The parent node is the boundary point, towards the two points s obs1 and s obs2 Connect and stay away from s nearest The parent node of the unidirectional expansion, otherwise, with two points s obs1 and s obs2 Connection for bidirectional expansion.

[0022] Step 4: If the mean point is in the free zone and is consistent with s nearest If the distance is greater than the threshold λ, then s nearest is the boundary point at the entrance of the narrow channel, and a new node is formed in the direction of the mean point; if the mean point is in the free zone and is connected to s nearest If the distance is less than λ, then the two sampling points s that are farthest apart in the free zone are free1 and s free2 The line goes towards the target point s goal Direction expansion.

[0023] The threshold λ is smaller than the distance Step_to_random.

[0024] The new node is generated by greedy expansion using binary method: the new node is generated under the joint attraction of the target point and the random sampling point, and the weight value is dynamically adjusted using binary method, and the initial step size is selected from the random sampling point s rand and the nearest random tree node s nearest The distance between or the nearest random tree node s nearestThe minimum value of the distance to the target point. If a collision still occurs when the step size is less than Step_to_random, the gravity of the target point is set to the step size Step_to_goal and the gravity of the random sampling point is set to the step size Step_to_random to expand the new node. If the collision detection result is still in a collision state, return to S42 to regenerate the random sampling point.

[0025] S45: Expand the generated new node twice toward the target point position, with the step size being the average value of the gaps between obstacles;

[0026] S46: Screening the feasibility of the new node expansion. If the new node is within the jurisdiction of the expanded node in the tree and has no collision with the expanded node, the new node to be expanded is considered an unnecessary expansion point, discarded, and returns to S42.

[0027] The jurisdiction area of ​​the expanded node in the tree: a spherical area with the expanded node as the center and the distance between the expanded node and its parent node as the radius;

[0028] S47: Determine whether the total number of nodes in the searched random tree exceeds the set total number of nodes. If so, delete the leaf nodes outside the ellipsoid with the initial point and the target point as the focus, and determine whether the target point position has been reached. If not, return to S42 and repeat the execution.

[0029] S5: Path optimization: The global path of the improved RRT*FN plan is set as the current path, and the cost value is calculated according to the path cost formula. Based on the principle of minimizing the cost value, the current path is judged and optimized using a method based on the triangle inequality. The cost formula is:

[0030]

[0031] Where P(s i ) represents node s i The actual path cost, that is, from the initial point to the node s i Total steps taken; 1 / F(s i ) represents the safety cost, F(s i ) is node s i The average distance between the location and nearby obstacles; T(s i ) represents the stability cost, whose value is s i The parent node moves to node s i The average change rate of each joint of the robotic arm; k1, k2, and k3 are the coefficients of the three costs, representing their proportion of the total cost;

[0032] The method based on triangle inequality is used to judge and optimize the current path: starting from the target point of the current path, if the node si The parent node of the parent node is s i The parent node of s is reduced under the condition of no collision. i When the cost is , that is, when the triangle inequality law is satisfied, then s i The parent node is adjusted to s i The parent node of the parent node, when s i The parent node of the parent node and s i If a collision occurs between i This node optimization is completed and enters the next s i Optimization of the parent node until the initial point s init , completing the optimization process.

[0033] S6: Smoothing: Use cubic B-spline curve to smooth the path and output the optimal path.

[0034] Compared with the prior art, the present invention has obvious beneficial effects. It can be seen from the above scheme that the sampling method of the present invention adopts a heuristic sampling method and introduces the sampling characteristics of the target area, so as to approach the target point position more quickly. The working space of the robotic arm is divided into four areas, and heuristic sampling is completed in these areas according to certain rules. The greedy expansion method and the quadratic expansion method of the dichotomy method are used to improve the convergence speed of the algorithm. In addition, the leaf nodes outside the ellipsoid are deleted first to solve the node removal problem, thereby obtaining a better search path and a faster search speed. When the node is expanded, a new node is generated according to the target gravity and the random point gravity, and the weight value is dynamically adjusted by the dichotomy method, and the new node is expanded to the target point twice to obtain a tree expansion direction closer to the target point more quickly. For boundary points, by expanding the local environment sampling boundary, the problems of narrow channel and stepped obstacle path planning can be effectively solved. In order to optimize the path, the redundant intermediate nodes are simplified based on the triangle inequality.

[0035] In summary, the present invention has the characteristics of satisfying the real-time optimal path condition and being adaptable to various scenarios.

[0036] The beneficial effects of the present invention are further illustrated below through specific implementation methods. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] Figure 1 It is a flow chart of the present invention;

[0038] Figure 2 It is a sampling space area division diagram of the present invention;

[0039] Figure 3 Generate a new node graph for the local environment sampling boundary expansion strategy of the present invention;

[0040] Figure 4Generate a new node graph by adopting the binary greedy expansion method of the present invention;

[0041] Figure 5 Expanding the feasibility graph for screening new nodes of the present invention;

[0042] Figure 6 This is a diagram for judging and optimizing the current path based on the triangle inequality method of the present invention;

[0043] Figure 7 This is a simulation diagram of an implementation case of the present invention. DETAILED DESCRIPTION

[0044] The following, in conjunction with the accompanying drawings and preferred embodiments, describes in detail the specific implementation, features and efficacy of a robot arm obstacle avoidance path planning method based on the RRT*FN algorithm proposed in the present invention.

[0045] See also Figure 1 The present invention provides a robot arm obstacle avoidance path planning method based on the RRT*FN algorithm, comprising the following specific steps:

[0046] S1: Establish a kinematic model for the robotic arm and perform kinematic analysis based on the established model. The robotic arm used satisfies the Pieper principle, so an analytical inversion method is used to complete the inverse kinematic solution of the robotic arm. The Pieper principle in robot kinematics states that the three adjacent joint axes of the robot intersect at a point or the three axes are parallel.

[0047] S2: Use the ball envelope method and the box envelope method to perform collision detection between the robot arm and obstacles and the robot arm links;

[0048] S3: Initialize the workspace of the robot and the environment information in the workspace, and give the initial point s of the robot movement init and the target point s to which the robot moves goal ;

[0049] S4: Use the improved RRT*FN algorithm applicable to multiple scenarios to plan the global path for the robot arm. The specific steps are as follows:

[0050] S41: Initialize the search tree and set the initial point s init Set as random tree root node T init Initialize the extended random tree region T_R and set the closest distance R between the node on the extended tree and the target point to s init to s goal The distance between them;

[0051] S42: Generate random sampling points s rand , the process is as follows:

[0052] S421: Divide the entire sampling space into Figure 2 The four areas shown are the target-oriented area, the target point area, the area where the expanded tree is located, and other unexplored areas;

[0053] S422: Through P goal_point ,P unexplored and P T_R The three parameters divide the [0,1] interval into four intervals, which represent the sampling probabilities of the target point area, the unexplored area, the target-oriented area, and the area where the expanded tree is located.

[0054] S423: If the probability of random generation is in [0,P goal_point ], then uniform sampling is performed within a sphere with the target point as the center and ε as the radius. If the probability of random generation is between [P goal_point ,P unexplored ], then sample in the unexplored area. If the probability of random generation is between [P T_R ,1], then the area where the expanded tree is located is sampled uniformly. If none of the above is true, random sampling points are sampled from the target guide area.

[0055] S43: traverse the random tree and search the random tree distance s rand nearest random tree node s nearest ;

[0056] S44: Determine the nearest random tree node s nearest Attributes, if s nearest Along the s nearest Point to s rand If the position point obtained by walking in the direction Step_to_random is within the obstacle and the boundary extension mark is 0, then judge s nearest For the unexpanded boundary points, a new node is generated by adopting the boundary expansion strategy based on local environment sampling, such as Figure 3 As shown;

[0057] S441: In order to obtain enough sampling points that meet the requirements to describe the local structure, s nearest As the center, each dimension coordinate is independently formed by ±Step_to_random nearest There are twice as many sampling points as the dimension, and then the previous sampling method is repeated with these sampling points as the center, but this time the coordinates of each dimension change by ±Step_to_random / 2;

[0058] S442: Obtain sampling point set S sampling_points Then divide these points into the free area point set S loc_free and the obstacle point set S loc_obs , then calculate the mean point s of the obstacle area point set avepoint ;

[0059] S443: If the mean point is within the obstacle, the new point will be the two points s farthest apart along the obstacle area point set. obs1 and s obs2 The connection is extended in one or two directions, depending on the nearest Is the parent node of the boundary point that has been extended? If so, move away from s along the line connecting the two points. nearest The unidirectional expansion of the parent node of . If the mean point is within the obstacle and the above expansion fails, the distance s in the free area is taken. nearest The farthest point is used as the new point.

[0060] S444: If the mean point is in the free zone and is equal to s nearest If the distance is greater than a threshold λ slightly smaller than Step_to_random, the boundary point s is determined. nearest At the entrance of the narrow channel, the new point tries to move towards the mean point s avepoint Directional expansion, if the mean point is in the free zone and is consistent with s nearest If the distance is less than λ, the line connecting the two farthest points in the free zone will be extended toward the target point.

[0061] S45: Determine the nearest random tree node s nearest Attributes, if s nearest The boundary extension mark is 1, then judge s nearest For the extended boundary points, considering that it is the first choice to go out of the boundary area as much as possible, the expansion of the new node in this case is only in the direction away from the obstacle, that is, the mean point s of the obstacle area point set in the local sampling avepoint Pointing to the direction of the nearest neighbor node, the step size is set to Step_to_random.

[0062] S46: Determine the nearest random tree node s nearest If neither S44 nor S45 conditions are met, a new node is generated by greedy expansion using binary search, such as Figure 4 As shown, the generation of new nodes is generated under the joint action of the target gravity and the random point gravity, and the weight value is dynamically adjusted using the dichotomy method. The initial step size selects the minimum value of the distance between the random point and the nearest node or the distance between the nearest node and the target point. The dichotomy is continued until the step size is just less than Step_to_random. If there is still a collision, the target gravity size is set to Step_to_goal and the random point gravity size is set to Step_to_random to expand the new node. If the collision detection result is still in a collision state, return to S42 to regenerate the random point.

[0063] S47: Expand the new node generated above twice toward the target point, with the step size being the average value of the gaps between obstacles.

[0064] S48: Screening the feasibility of new node expansion, such as Figure 5 As shown, if the new node is in the jurisdiction of the expanded node in the tree, that is, the circular area of ​​the distance between the expanded node and its parent node (this is related to reselecting the parent node and rewriting the neighboring nodes s in the routine near The area involved in the calculation is different) and there is no collision with the expanded node, the new node to be expanded is regarded as an unnecessary expansion point, discarded and returns to S42;

[0065] S49: Determine whether the total number of nodes in the searched random tree exceeds a fixed value. If so, delete the leaf nodes outside the ellipsoid with the initial point and the target point as the focus, and determine whether the target point position has been reached. If not, return to S42 and repeat the execution.

[0066] S5: Set the global path PATH planned by the improved RRT*FN as the current path, and use the triangle inequality method to judge and optimize the current path according to the principle of minimizing the path cost formula 1, such as Figure 6 As shown;

[0067] The formula 1 is:

[0068]

[0069] Where P(s i ) means s i The actual path cost of the point is from the initial point to s i The total step length of the point, 1 / F(s i ) represents the safety cost and F(s i ) value is s i The average distance between the location and nearby obstacles, T(s i ) represents the stability cost, whose value is s i The parent node moves to the current point s i The average change rate of each joint of the robotic arm. k1, k2, and k3 are the coefficients of the three sub-costs, representing their proportion to the total cost.

[0070] S51: Start optimization from the target point of the current path. If s i The parent node of the parent node is s i The parent node of the node reduces s under the condition of no collision i When the cost is , that is, when the triangle inequality law is satisfied, then s i The parent node is adjusted to s i The parent node of the parent node, when s i The parent node of the parent node and s i If a collision occurs between i This node optimization is completed and enters the next si Optimize the parent node, and so on until s init The entire optimization process ends at the initial node;

[0071] S52: After completing the entire round of optimization, the corresponding nodes can be inserted into the newly obtained path to further repeat the above triangle inequality optimization process. The corresponding insertion point is obtained by moving a certain distance along the obtained path with the point to be optimized as the base point. The distance is the average radius of the volume of all obstacles represented by the sphere and the distance from the target point to the point to be optimized s i The minimum value is half the length of the straight line between them.

[0072] S6: Use the cubic B-spline curve to smooth the path and generate the final, currently optimal path.

[0073] S7: Control the robot arm to move to the target point position through the planned path solution.

[0074] Performance Analysis:

[0075] A three-dimensional obstacle avoidance environment was constructed in Python, and a three-dimensional simulation comparison was performed with the RRT*FN and RRT-Connect algorithms. To test the adaptability of the proposed method to complex narrow channels in a three-dimensional environment, a small ball with a radius of 2 cm was set as the navigation object, the safety distance delta was 0.2 cm, the entire spatial layout was 100 cm × 100 cm × 100 cm, and the minimum gap between narrow channels was 5 cm. A total of 1000 sets of experiments were conducted in each environment, and the results are shown in Table 1. The shortest distance between the final path and the obstacle was 0.3 cm. Due to the use of a heuristic sampling algorithm, the proposed method is more accurate and effective in the use of sampling points. In addition, the combination of the greedy expansion method and the quadratic expansion strategy can make the tree converge to the target point position more quickly, while generating the optimal expansion process path. In particular, a special and effective boundary expansion method is used for boundary points. Finally, the path is optimized based on the triangle inequality. Therefore, the proposed method not only ensures real-time performance but also produces better paths than other algorithms in various scenarios.

[0076] Table 1 Three-dimensional experimental results

[0077]

[0078] In order to verify the feasibility of the manipulator path planning method of the present invention, the proposed algorithm is applied to the six-degree-of-freedom serial manipulator controlled by the ROS platform. The preset hyperparameters such as safety distance and extended step size are as follows: the starting point is (0, 0, 168), the target point is (15, 22, 60), the general step size Step_to_random = 0.8 cm, Step_to_goal = 0.4 cm, P goal_point=0.1, initially P T_R =0.95, P unexplored = 0.2, the maximum number of nodes is fixed to 2000, and the maximum number of iterations is 10000.

[0079] like Figure 7 As shown in the figure, the path planning method of the present invention guides the manipulator to complete the movement from the starting position to the specified position on the first layer of the shelf where the workpieces are stored, and completes the workpiece sorting task T. For task T, 500 experiments were conducted using RRT*, RRT*FN, RRT-Connect and the method of the present invention, and the data provided are shown in Table 2. During the entire planning process of completing task T, the running time of the manipulator should not exceed 80s; otherwise, the planning is deemed invalid. Judging from the data in Table 2, the planning efficiency of the method of the present invention is higher, and each test is successful. The smaller the average rotation angle of the entire joint planned by the manipulator at one time, the stronger the path optimization ability and energy-saving ability of the method. The difference between the maximum rotation angle and the average rotation angle is small, indicating that the movement of the manipulator is more stable.

[0080] Table 2 Results of completing task T under various control methods in ROS simulation

[0081]

[0082] The above description is merely a preferred embodiment of the present invention and does not constitute any form of limitation to the present invention. Any simple modification, equivalent change and modification made to the above embodiment based on the technical essence of the present invention without departing from the content of the technical solution of the present invention shall still fall within the scope of the technical solution of the present invention.

Claims

1. A robot arm obstacle avoidance path planning method based on the RRT*FN algorithm, characterized by: The method comprises the following steps: S1: Establishing a kinematic model: establishing a kinematic model of the robotic arm, and performing kinematic analysis of the robotic arm based on the kinematic model of the robotic arm; S2: Collision detection: performing collision detection between the robotic arm and obstacles and the robotic arm connecting rod; S3: Initialize the workspace: Initialize the workspace where the robot moves and the environment information in the workspace. The workspace is divided into a free area and an obstacle area, and the initial point s of the robot movement is given. init and the target point s to which the manipulator moves goal ; S4: Using the improved RRT*FN algorithm applicable to multiple scenarios to plan a global path for the robotic arm, the specific steps are as follows: S41: Initialize the search tree and set the initial point s init Set as random tree root node T init Initialize the extended random tree region T_R and set the closest distance R between the node on the extended tree and the target point to s init to s goal The distance between them; set the total number of nodes; S42: Generate random sampling points s rand , the process is as follows: S421: Calibrate the entire sampling space to the interval [0,1], and set the probability value P goal_point , probability value P unexplored and probability value P T_R , divide the [0,1] interval into four sub-intervals, namely the target point interval [0,P goal_point ]、Unexplored interval[P goal_point ,P unexplored ], goal-oriented interval [P unexplored ,P T_R ]、The interval where the expanded tree is located [P T_R ,1]; S422: If the probability of random generation is in [0,P goal_point ], then the sample is uniformly collected within a sphere with a certain radius and the target point as the center; if the probability of random generation is between [P goal_point ,P unexplored ], then sample in the unexplored area; if the probability of random generation is between [P T_R ,1], then uniform sampling is performed in the area where the expanded tree is located; if none of the above is true, random sampling points are sampled from the target-oriented interval; S43: traverse the random tree and search the random tree distance s rand nearest random tree node s nearest ; S44: Determine the nearest random tree node s nearest Attributes: If s nearest Along the s nearest Point to s rand The position point obtained by walking distance Step_to_random in the direction is located within the obstacle and the boundary extension mark is 0, then judge s nearest is an unexpanded boundary point, and a new node is generated by adopting a boundary expansion strategy based on local environment sampling; if s nearest The boundary extension mark is 1, then judge s nearest For the extended boundary points, the expansion of the new node adopts a safe expansion strategy in the direction away from the obstacle, that is, the mean point of the obstacle area point set in the local sampling points to the nearest random tree node s nearest direction, at this time the step size is set to Step_to_random; otherwise, a new node is generated by greedy expansion using binary search: the new node is generated under the joint force of the target point gravity and the random sampling point gravity, and the weight value is dynamically adjusted using binary search. The initial step size is the minimum value of the distance between the random sampling point srand and the nearest random tree node snearest or the distance between the nearest random tree node snearest and the target point. When a collision occurs when the step size is less than Step_to_random, the target point gravity is set to the step value Step_to_goal and the random sampling point gravity is set to the step value Step_to_random to expand the new node. If the collision detection result is still in a collision state, return to S42 to regenerate the random sampling point. The method of generating a new node based on the local environment sampling boundary expansion strategy includes the following steps: (1) In order to obtain enough sampling points that meet the requirements to describe the local structure, s nearest As the center, each dimension of its spatial coordinates is independently taken to have a positive and negative distance ±Step_to_random, forming s nearest Then, with the obtained sampling point as the center, each dimension of its spatial coordinate is independently taken as half of the positive and negative distance ±Step_to_random / 2 to form a new sampling point; (2) Obtain the sampling point set S sampling_points Then these sampling points are divided into the free zone sampling point set S loc_free and the obstacle area sampling point set S loc_obs , then calculate the mean point s of the obstacle area sampling point set avepoint ; (3) If the mean point is within the obstacle, the two sampling points s that are farthest apart along the obstacle area point set are obs1 and s obs2 The line is extended in one direction or two directions to form a new node; if the mean point is within the obstacle and the one-way or two-way extension fails, the distance s in the free zone is taken nearest The farthest point is used as the new node; (4) If the mean point is in the free zone and is equal to s nearest If the distance is greater than the threshold λ, then s nearest is the boundary point at the entrance of the narrow channel, and a new node is formed in the direction of the mean point; if the mean point is in the free zone and is connected to s nearest If the distance is less than λ, then the two sampling points s that are farthest apart in the free zone are free1 and s free2 The line goes towards the target point s goal Directional expansion; S45: Expand the generated new node twice toward the target point position, with the step size being the average value of the gaps between obstacles; S46: Screening the feasibility of the new node expansion. If the new node is within the jurisdiction of the expanded node in the tree and has no collision with the expanded node, the new node to be expanded is considered an unnecessary expansion point, discarded, and returns to S42. The jurisdiction area of ​​the expanded node in the tree: a spherical area with the expanded node as the center and the distance between the expanded node and its parent node as the radius; S47: Determine whether the total number of nodes in the searched random tree exceeds the set total number of nodes. If so, delete the leaf nodes outside the ellipsoid with the initial point and the target point as the focus, and determine whether the target point position has been reached. If not, return to S42 and repeat the execution. S5: Path optimization: The global path of the improved RRT*FN plan is set as the current path, and the cost value is calculated according to the path cost formula. Based on the principle of minimizing the cost value, the current path is judged and optimized using a method based on the triangle inequality. The cost formula is: Where P(s i ) represents node s i The actual path cost, that is, from the initial point to the node s i Total steps taken; 1 / F(s i ) represents the safety cost, F(s i ) is node s i The average distance between the location and nearby obstacles; T(s i ) represents the stability cost, whose value is s i The parent node moves to node s i The average change rate of each joint of the robotic arm; k1, k2, and k3 are the coefficients of the three costs, representing their proportion of the total cost; S6: Smoothing: Use cubic B-spline curve to smooth the path and output the optimal path.

2. The robot arm obstacle avoidance path planning method based on the RRT*FN algorithm according to claim 1, characterized in that: In step S1, the robotic arm satisfies the Pieper principle, and an analytical method is used to obtain an inverse kinematic solution for the robotic arm.

3. The robot arm obstacle avoidance path planning method based on the RRT*FN algorithm according to claim 1, characterized in that: In step S44, in the step (3) of generating new nodes based on the local environment sampling boundary expansion strategy, the two points s that are farthest apart along the obstacle area point set are obs1 and s obs2 Connection unidirectional or bidirectional extension: If s nearest The parent node is the boundary point, towards the two points s obs1 and s obs2 Connect and stay away from s nearest The parent node of the unidirectional expansion, otherwise, with two points s obs1 and s obs2 Connection for bidirectional expansion.

4. The robot arm obstacle avoidance path planning method based on the RRT*FN algorithm according to claim 1, characterized in that: In step S44, in the step (4) of generating a new node based on the local environment sampling boundary expansion strategy, the threshold λ is less than the distance Step_to_random.

5. The robot arm obstacle avoidance path planning method based on the RRT*FN algorithm according to claim 1, characterized in that: In step S5, the method based on triangle inequality is used to judge and optimize the current path: starting from the target point of the current path, if the node s i The parent node of the parent node is s i The parent node of s is reduced under the condition of no collision. i When the cost is , that is, when the triangle inequality law is satisfied, then s i The parent node is adjusted to s i The parent node of the parent node, when s i The parent node of the parent node and s i If a collision occurs between i This node optimization is completed and enters the next s i Optimization of the parent node until the initial point s init , completing the optimization process.

6. The robot arm obstacle avoidance path planning method based on the RRT*FN algorithm according to claim 1, characterized in that: In step S2, the collision detection adopts the sphere envelope method and the box envelope method.

Images