Robot intelligent sorting path planning method and device for stacked parts in charging basket

Abstract

The invention relates to a robot intelligent sorting path planning method and a robot intelligent sorting path planning device for stacked parts in a charging basket, an RRT * algorithm is combined with an artificial potential field algorithm, random search of the RRT * algorithm in a barrier-free free space can be reduced by utilizing an artificial potential field, and meanwhile, in a complex barrier environment close to a part pile, the robot intelligent sorting path planning method and the robot intelligent sorting path planning device have the advantages that the random search of the RRT * algorithm in the barrier-free free space can be reduced; and an RRT * algorithm is used for accurate collision-free path planning, so that the problems of low path planning precision and local minimum of an artificial potential field algorithm are avoided. On the basis, the RRT * algorithm is improved, the problems of many redundant nodes, low planning efficiency and poor path quality existing in path planning of the RRT * algorithm are solved, meanwhile, the improved RRT * algorithm is combined with the artificial potential field algorithm, the planning scale is further reduced, and random search of the RRT * algorithm in a free space is reduced. According to the method and device provided by the invention, an efficient and accurate collision-free picking path planning scheme can be provided for industrial application of mechanical arm material frame sorting.

Description

Technical Field

[0001] The present invention relates to the technical field related to robot arm path planning, and in particular to a method and device for intelligently sorting paths of robot parts stacked in a basket. Background Art

[0002] The purpose of industrial robot path planning is to set the robot's working path according to the task and operating environment. Nowadays, industrial robots are widely used in target sorting scenarios, but in most cases they are carried out in a structured environment by manual teaching, which has the defects of simple application scenarios and weak robot perception ability. When the sorting object facing the robot arm is the scattered and stacked parts in the material basket, the path planning of the end of the robot arm needs to be completed before sorting. If there is no accurate sorting path planning, the end of the robot arm and the target parts will interfere with or even collide with the wall of the material frame and the remaining parts in the frame during the removal process, which will seriously affect the success rate and safety of sorting. If the sorting path is set by manual teaching, it cannot be applied to the different placement postures of each part in the material basket, and lacks practicality and flexibility. Therefore, an accurate and flexible robot arm path planning method is needed to meet the industrial application needs of robot arm material frame sorting.

[0003] The rapidly-exploring random tree (RRT) algorithm is a typical sampling-based motion planning algorithm. When solving high-dimensional space path planning problems such as multi-axis robot path planning, this algorithm has the characteristics of low computational complexity and good versatility. Therefore, it is widely used in the research of robot path planning. Based on this, foreign scholars proposed an RRT* algorithm, as shown in the schematic diagram Figure 3 As shown in the figure, by introducing parent node reselection and rewiring on the basis of the RRT algorithm, it is guaranteed that the optimal solution or near-optimal solution can be searched within a limited time, which improves the path optimization capability of the RRT algorithm and reduces the path cost. However, since the RRT* algorithm performs global random sampling of the state space during the search, the algorithm still has the defects of many redundant nodes, low planning efficiency, and poor path quality.

[0004] The artificial potential field algorithm is a commonly used algorithm for robot path planning. It is based on the concept of simulated force field and guides the robot's movement in the environment by setting the gravitational force generated by the target node and the repulsive force generated by the obstacle point. When the obstacle layout is relatively uniform, the algorithm has good robustness and running speed, but in a complex obstacle environment, the algorithm is prone to fall into a local minimum. Summary of the invention

[0005] The purpose of the present invention is to solve at least one of the deficiencies of the prior art and to provide a method and device for intelligent sorting path planning of a robot for stacked parts in a basket.

[0006] In order to achieve the above object, the present invention adopts the following technical solutions:

[0007] Specifically, a method for intelligent sorting path planning of a robot for stacked parts in a basket is proposed, including the following:

[0008] The planning scene point cloud is obtained through a 3D camera, and the planning space is determined based on it;

[0009] Initialize the starting node and target node of the planned path;

[0010] Constructing an artificial potential field based on the scene point cloud and the target node;

[0011] Based on the artificial potential field, predicting a first path connecting the starting node and the target node;

[0012] Based on the predicted first path, updating the target node;

[0013] Improve the search step size and target guidance probability threshold of the RRT* algorithm through adaptive initialization of the target part's starting position;

[0014] Based on the improved RRT* algorithm, a path planning is performed from the starting node to the updated target node to obtain a planning result;

[0015] Performing path de-redundancy processing on the planning result to obtain a de-redundant path;

[0016] Performing path smoothing processing on the de-redundant path to obtain a smoothed picking path;

[0017] The obtained smooth picking path is converted into a robot arm script language through a host computer, and the robot arm is driven to complete the picking of the target part along the smooth picking path.

[0018] Further, specifically, constructing an artificial potential field based on the scene point cloud and the target node includes:

[0019] Calculate the radius R of the enclosing sphere of the end of the robot arm, which includes the end-of-arm pickup tool and the currently picked-up part, and As the range of action of the repulsive field P0; first use the formula To construct the gravitational field for the planning space, ξ is the gravitational scale factor, which is set to 0.5, and ρ(q,q goal ) represents the current node q and the target node q goal The distance between them; then through the formula To construct a repulsive field, i.e., an artificial potential field, for the planning space, η represents the repulsive field scale factor, which is set to 5000, and ρ(q,q obs ) represents the obstacle point qobs That is, the distance from the point in the scene point cloud to the current node q.

[0020] Further, specifically, based on the artificial potential field, predicting a first path connecting the start node and the target node includes:

[0021] Initialization parameters, including the expansion step size Step size , maximum number of iterations Epoch,

[0022] Initialize the starting node X start With the target node X goal As the starting point and end point of the artificial potential field prediction path,

[0023] Initialize the first path node list and set the starting node X start Add to the node list,

[0024] Take the node most recently added to the node list as the current point, and derive the formula of the artificial potential field to obtain the gravity calculation formula And the repulsion calculation formula The gravity calculation formula and repulsion calculation formula can be used to calculate the magnitude of the gravity and repulsion of the current point in the three directions of xyz, and at the same time multiply the resultant force in each direction by the step length Step size The coordinate changes in each direction can be obtained. After superimposing the coordinate changes on the current point, a new node can be obtained and the node is added to the node list. Finally, the distance from the newly added node to the target node is determined. If it is less than the distance threshold, the predicted path is found and the node list is output. Otherwise, this step is repeated until the predicted result is found.

[0025] Further, specifically, based on the predicted first path, updating the target node includes:

[0026] Based on the pose of the starting node and the pose of the target node, the pose corresponding to each path point in the predicted path is calculated by pose linear interpolation;

[0027] Traverse each path point in the predicted path in turn, connect it directly to the target node, and determine whether the direct path collides, until the path point X corresponding to the path without collision is found. direct , and record the corresponding direct path Path direct ;

[0028] Update the planned target node to the above path point X direct , denoted as X goal_new .

[0029] Further, specifically, the search step size and target guidance probability threshold of the RRT* algorithm are improved by adaptively initializing the starting pose of the target part, including:

[0030] Calculate the angle θ between the Z-axis direction of the starting node and the vertical direction, using the formula Bias = 4.377θ 2 -4.107θ+1Calculate the target guidance probability P of the random tree thresh ;

[0031] Starting from n=2, the pose corresponding to the first point in the n equally divided points is calculated by linear interpolation of the poses of the starting node and the target node, and the collision of the end of the robot arm in this pose is checked. When the end of the robot arm does not collide with the side wall of the material frame, and the median penetration depth of the end of the robot arm colliding with the remaining parts in the material basket is less than 2mm, record the value of n, calculate the distance dist between the starting node and the target node, and As the search step of the random tree size .

[0032] Further, specifically, based on the improved RRT* algorithm, path planning from the starting node to the updated target node is performed to obtain a planning result, including:

[0033] Step 610: Sample a sampling node X in the planning space based on the target guidance strategy. sample , find the point X on the random tree that is closest to the sampled node nearest , then from X nearest Start and head towards sampling node X sample Direction expansion step Step size , get the new node X new ;

[0034] Step 620: Determine the obtained node X new Whether the node rejection mechanism is satisfied, if not, return to step 610 to resample, if satisfied, proceed to the next step;

[0035] Step 630: Check Node X new If the inverse solution result and the collision situation of the node X do not meet the validity requirements, then return to step 610 to resample. Otherwise, select new The random tree node with the lowest path cost after connection is taken as X new The parent node of the new node X new Add to random tree;

[0036] Step 640: Add the node X newly added to the random tree newAs the center, connect the rest of the nodes within a certain range to the center point. If the cost of the connected nodes decreases, the center point X new As the parent node of this node;

[0037] Step 650: Determine whether the random tree has reached the set target node based on the distance between the newly added node and the target node. If it has reached the target node, output the original path Path raw Otherwise, return to step 610 to resample until the random tree is connected to the target node.

[0038] Further, specifically, performing path de-redundancy processing on the planning result to obtain a de-redundant path includes:

[0039] Step 710: Initialize the pruning path result Path pruned , change the original path Path raw The first node P start Add to Path pruned In the example, let P end As Path raw Medium start The next node of

[0040] Step 720: P start With P end The direct path is used for collision detection;

[0041] Step 730: Determine the collision detection result. If no collision occurs, set the original path Path raw The next node in P end , execute step 720, otherwise record the node P end In the original path Path raw The previous node P in end-1 , and add it to Path pruned Then the starting node P start Update to P end-1 Then continue to execute step 720, search for key points on the path backward until the original path Path is traversed. raw All key points of the Path pruned .

[0042] Further, specifically, performing path smoothing processing on the redundant path to obtain a smoothed picking path includes:

[0043] Step 810: Remove redundant paths Path pruned The path points are used as control points of the quasi-uniform cubic B-spline, so as to fit a curve with smoothly changing path point positions;

[0044] Step 820: traverse the control points of the B-spline curve, find the path point closest to the control point in the B-spline curve, and initialize the posture of the path point to the posture of the corresponding control point;

[0045] Step 830: Divide the smooth curve into multiple segments through the path points with initialized posture information, and use the starting posture and the end posture of each path segment to interpolate the posture corresponding to the intermediate smooth path point, so as to obtain the improved RRT* algorithm path planning result Path after smoothing. smooth ;

[0046] Step 840: Path according to the collision-free direct path direct , linearly interpolate at certain intervals to obtain the position and posture of the intermediate nodes of the path;

[0047] Step 850: The Path obtained in step 830 smooth The direct path Path obtained in step 840 direct Merge to generate the final smooth picking path.

[0048] The present invention also proposes a device for intelligent path planning of robot sorting of stacked parts in a basket, comprising the following:

[0049] A data acquisition module is used to acquire the planning scene point cloud through a three-dimensional camera and determine the planning space based on it;

[0050] A first initialization module is used to initialize the starting node and the target node of the planned path;

[0051] An artificial potential field construction module, used to construct an artificial potential field based on the scene point cloud and the target node;

[0052] A path prediction module, used for predicting a first path connecting the starting node and the target node based on the artificial potential field;

[0053] A target node updating module, used for updating the target node based on the predicted first path;

[0054] The second initialization module is used to adaptively initialize the search step size and target guidance probability threshold of the improved RRT* algorithm through the starting position of the target part;

[0055] A path planning module, used to perform path planning from a starting node to an updated target node based on the improved RRT* algorithm to obtain a planning result;

[0056] A path de-redundancy module, used for performing path de-redundancy processing on the planning result to obtain a de-redundant path;

[0057] A path smoothing module, used for performing path smoothing processing on the de-redundant path to obtain a smoothed picking path;

[0058] The robot control module is used to convert the obtained smooth picking path into a robot script language through a host computer, and drive the robot to complete the picking of the target part along the smooth picking path.

[0059] The beneficial effects of the present invention are:

[0060] The present invention proposes a method and device for intelligent sorting path planning of stacked parts in a basket by a robot, which has at least the following advantages:

[0061] 1. Based on the artificial potential field method, the position information of a series of discrete picking path nodes is predicted. By predicting the direct connection between the path node and the target node and judging the validity of the position of the intermediate node of the direct connection path, a collision-free path from the middle point of the predicted path to the target node can be quickly found, avoiding the random search of the improved RRT* algorithm in the obstacle-free area and improving the path planning efficiency.

[0062] 2. After updating the target node to the intermediate node of the predicted path, the distance from the start node to the target node is further reduced, and the expansion scale of the random tree is reduced, thereby reducing the amount of calculation required for planning and speeding up the search for a feasible path solution.

[0063] 3. The proximity of the starting node and the target node makes the planned original path have fewer path nodes, which reduces the number of nodes that need to be processed in the path pruning step, reduces the amount of calculation, and improves the efficiency of path optimization.

[0064] 4. The improved RRT* algorithm adds an adaptive step size and adaptive target guidance probability calculation strategy based on the starting position information of the part, so that the algorithm can automatically calculate the appropriate search step size and target guidance probability threshold according to the starting state of the part to meet the adaptation to various placement positions of the part, thereby improving the stability and adaptability of the algorithm planning.

[0065] 5. The addition of the target-guided sampling strategy enables the random tree to bypass obstacle areas and avoid falling into local minima when expanding, and also quickly approach the target node, achieving a balance between the exploratory and purposeful nature of the algorithm.

[0066] 6. The introduction of a sampling node rejection mechanism based on the posture constraint of the robot end can effectively reduce the number of inverse solution calculations and collision detection without affecting the planning success rate, thereby reducing the computational cost of the planning process and shortening the overall planning time.

[0067] 7. Through path pruning and smoothing, a shorter and smoother path can be generated, which helps to improve the sorting efficiency and movement smoothness of the robot arm. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] The above and other features of the present disclosure will become more apparent by describing in detail the embodiments shown in the accompanying drawings. The same reference numerals in the accompanying drawings of the present disclosure represent the same or similar elements. Obviously, the accompanying drawings described below are only some embodiments of the present disclosure. For those skilled in the art, other accompanying drawings can be obtained based on these accompanying drawings without creative work. In the accompanying drawings:

[0069] Figure 1 It is a schematic diagram of the overall technical solution of the present invention;

[0070] Figure 2 A schematic diagram of a hardware device platform used in the present invention;

[0071] Figure 3 Schematic diagram of the standard RRT* algorithm;

[0072] Figure 4 It is a flow chart of a method for intelligent sorting path planning of a robot for stacked parts in a basket;

[0073] Figure 5 Schematic diagram of predicting sorting paths and updating target nodes through artificial potential fields;

[0074] Figure 6 Schematic diagram of planning using the improved RRT* algorithm after updating the target node;

[0075] Figure 7 It is a sub-flow chart of step S104;

[0076] Figure 8 It is a sub-flow chart of step S105;

[0077] Fig. 9 It is a sub-flow chart of step S106;

[0078] Fig.10 It is a sub-flow chart of step S107;

[0079] Fig.11 is a schematic diagram of step S108;

[0080] Fig.12 It is a sub-flow chart of step S108;

[0081] Fig.13 It is a sub-flow chart of step S109;

[0082] Fig.14 This is a comparison diagram of the original path and the pruned path;

[0083] Fig.15An example of path smoothing results in one implementation

[0084] Fig.16 This is an example diagram of the robot arm simulation results. DETAILED DESCRIPTION

[0085] The following will be combined with the embodiments and drawings to clearly and completely describe the concept, specific structure and technical effects of the present invention, so as to fully understand the purpose, scheme and effect of the present invention. It should be noted that the embodiments in this application and the features in the embodiments can be combined with each other without conflict. The same reference numerals used throughout the drawings indicate the same or similar parts.

[0086] In view of the problems existing in the RRT* algorithm, combined with the industrial application scenario of robotic arm material frame sorting, Example 1, refer to Figure 1 The present invention proposes a method for intelligent sorting path planning of stacked parts in a basket by a robot, comprising the following steps:

[0087] The planning scene point cloud is obtained through a 3D camera, and the planning space is determined based on it;

[0088] Initialize the starting node and target node of the planned path;

[0089] Constructing an artificial potential field based on the scene point cloud and the target node;

[0090] Based on the artificial potential field, predicting a first path connecting the starting node and the target node;

[0091] Based on the predicted first path, updating the target node;

[0092] Improve the search step size and target guidance probability threshold of the RRT* algorithm through adaptive initialization of the target part's starting position;

[0093] Based on the improved RRT* algorithm, a path planning is performed from the starting node to the updated target node to obtain a planning result;

[0094] Performing path de-redundancy processing on the planning result to obtain a de-redundant path;

[0095] Performing path smoothing processing on the de-redundant path to obtain a smoothed picking path;

[0096] The obtained smooth picking path is converted into a robot arm script language through a host computer, and the robot arm is driven to complete the picking of the target part along the smooth picking path.

[0097] In the hardware part, the computer is used as the host computer to exchange data and control with the 3D camera and the robotic arm through the network switch. Figure 2 As shown;

[0098] In this embodiment 1, by combining the RRT* algorithm with the artificial potential field algorithm, the artificial potential field can be used to reduce the random search of the RRT* algorithm in the unobstructed free space, and at the same time, in the complex obstacle environment near the parts pile, the RRT* algorithm is used to perform accurate collision-free path planning, thereby avoiding the low planning path accuracy of the artificial potential field algorithm and the problem of falling into local minima. Based on this, the present invention provides a method and device for intelligent sorting path planning of stacked parts robot in a material basket. In the algorithm part, by improving the RRT* algorithm, the problems of many redundant nodes, low planning efficiency and poor path quality existing in the RRT* algorithm for path planning are solved. At the same time, the improved RRT* algorithm is combined with the artificial potential field algorithm to further reduce the planning scale and reduce the random search of the RRT* algorithm in the free space; in the hardware part, the computer is used as a host computer, and data is exchanged and controlled with the three-dimensional camera and the mechanical arm through the network switch. The three-dimensional camera is used as a scene perception tool to obtain the point cloud of the object to be picked up, and the sorting path is planned by the aforementioned algorithm, and the host computer drives the mechanical arm to pick up. Based on this method and device, an efficient and accurate collision-free picking path planning solution is provided for the industrial application of robotic arm material frame sorting.

[0099] The algorithm flow is as follows Figure 4 As shown, it includes steps S101 to S109.

[0100] Planning initialization steps:

[0101] S101: Obtain a scene point cloud by shooting with a 3D camera, extract the box point cloud in the scene, and construct a planning space according to the size of the box point cloud bounding box.

[0102] S102: Obtain the placement pose of the target part to be picked up in the material frame through the pose estimation algorithm, and initialize it as the starting node X of the planning start ; Set the state of the robot arm when picking is completed through manual teaching, and initialize the position of the target part corresponding to this state as the target node X goal .

[0103] S103: Calculate the radius R of the enclosing sphere of the end of the robot arm (including the pick-up tool at the end of the robot arm and the currently picked-up part), and As the range of action of the repulsive field P0; first use the formula To construct the gravitational field for the planning space, ξ is the gravitational scale factor, which is set to 0.5, and ρ(q,q goal ) represents the current node q and the target node q goal The distance between them; then through the formula To construct a repulsive field for the planning space, η represents the repulsive field scale factor, which is set to 5000, and ρ(q,q obs ) represents the obstacle point q obs (i.e. the point in the scanned point cloud) to the distance from the current node q.

[0104] S104: Based on the constructed artificial potential field and the starting node and target node of the planning task, the prediction of the picking path is completed. The specific process is as follows: Figure 7 As shown, the steps are as follows:

[0105] Initialization parameters, including the expansion step size Step size (set to 5), maximum number of iterations Epoch (set to 15000).

[0106] Initialize the starting node and the target node, and obtain the starting node X in step S102 start With the target node X goal As the starting point and end point of the artificial potential field prediction path.

[0107] Initialize the predicted path node list and add the starting node to the node list.

[0108] The node added to the node list the most recently is taken as the current point, and the gravity calculation formula is obtained by deriving the formula of the artificial potential field in S103 And the repulsion calculation formula This formula can be used to calculate the magnitude of the gravitational force and repulsive force on the current point in the three directions of xyz, and multiply the resultant force in each direction by the step length Step size The coordinate changes in each direction can be obtained. After superimposing the coordinate changes on the current point, a new node can be obtained and the node is added to the node list. Finally, the distance from the newly added node to the target node is determined. If it is less than the threshold, the predicted path is found and the node list is output. Otherwise, this step is repeated until the predicted result is found.

[0109] S105: Based on the predicted path result, update the target node of the planning task, as shown in the schematic diagram Figure 5 As shown, the process is as Figure 8 As shown, the specific steps are as follows:

[0110] Based on the starting node X start With the target node X goal The pose information of each predicted path point is obtained by linear interpolation.

[0111] The starting node of the predicted path is taken as the current checkpoint X, and the point is aligned with the target node X. goalMake a direct connection, calculate the length of the direct connection path, and perform linear interpolation between the current checkpoint and the target node at intervals of 5 to obtain a series of intermediate nodes X inter , use the posture of the intermediate node to update the posture of the end of the robot arm (including the picking mechanism and the target part), and determine whether the end does not collide in each posture. If no collision occurs, the current checkpoint X is used as the new target node X goal_new Otherwise, the next node of the predicted path is used as the current checkpoint and this step is repeated until a checkpoint that meets the conditions is found.

[0112] like Figure 6 As shown in Figure 2, after completing the above planning initialization steps, the new target node X is found through the artificial potential field. goal_new To the original target node X goal The collision-free direct path is then completed based on the improved RRT* algorithm to complete the starting node X start To the new target node X goal_new Path planning.

[0113] Random Tree Search Steps:

[0114] S106: Adaptively initialize the search step size and target guidance probability threshold of the improved RRT* algorithm through the starting position of the target part. The process is as follows Fig. 9 As shown, the specific steps are as follows:

[0115] Calculate the angle θ between the Z-axis direction of the starting node and the vertical direction.

[0116] Use the adaptive target guidance probability calculation formula Bias = 4.377θ 2 -4.107θ+1 (through the point (0,1), point point Calculate) to obtain the target guidance probability Bias.

[0117] Starting from n=2, for the starting node X start With the current target node X goal_new Perform pose linear interpolation calculation to obtain the pose corresponding to the first point among the n equally divided points, and check the collision of the end of the robot arm (including the end picking tool and the currently picked target part) in this pose. When the end of the robot arm does not collide with the side wall of the material frame, and the median penetration depth of the end colliding with the remaining parts in the material basket is less than 2mm, record the value of n, calculate the distance dist between the starting node and the target node, and As the search step of the random tree size , if the above collision condition is not met, set n=n+1 and repeat this step.

[0118] S107: After determining the search step length and the target guidance probability, the path planning from the starting node to the current target node is started based on the improved RRT* algorithm. The process is as follows: Fig.10 As shown, the specific steps are as follows:

[0119] Randomly pick a number P between [0,1] and determine the relationship between P and Bias. When P is greater than Bias, the algorithm directly moves the target node X to the next node. goal_new As sampling node X sample Otherwise, randomly sample a point X in the planning space rand As X sample .

[0120] According to the sampling point X sample , find the distance X in the random tree sample The nearest point X nearest , and from X nearest Set off towards X sample Direction expansion Step size Get node X new .

[0121] Compute Node X new The angle α between the corresponding posture normal vector and the vertical direction vector is , where θ represents the angle between the normal direction of the starting node and the vertical direction. If α is greater than Then we consider that node X new If invalid, return to the previous step for resampling; otherwise, the node is valid and proceed to the next step.

[0122] Inverse solution check: The kinematic model of the robot arm is established by the Modified-DH parameter method, and the new node X is analyzed by the analytical method. new Perform inverse solution calculation. When there is at least one set of joint angles that meet the joint travel limit in the inverse solution, perform the next step of collision detection, otherwise return to step a) for resampling.

[0123] Collision detection: Construct the BVH model in the FCL library based on the STL format model of the robot end and the target object, use the BVH model to divide the object into the smallest primitives and generate collision objects, and then new Update the position and posture of the collision object, and detect whether the end tool of the robot arm and the picked object in this position collide with other objects in the space. If there is a collision, return to step a) to resample, otherwise proceed to the next step.

[0124] Parent node reselection: select a node in the random tree node list so that the current node X new After connecting to this node, X newThe corresponding path cost is the lowest, and the tree node is used as the current sampling node X new The parent node of the new node X new Added to the random tree.

[0125] Rewiring: With the newly added node X in the random tree new As the center, connect the random tree nodes within a certain range to the node respectively, and determine whether the path cost corresponding to the connected random tree node is reduced. If it is reduced, update the parent node of the tree node to node X. new .

[0126] Determine the node X newly added to the random tree new The distance to the target node. If the distance is less than a certain threshold, the random tree is considered to have expanded to the target node. At this time, the node is backtracked to generate the path result Path raw Otherwise, return to step a) to continue searching.

[0127] Path redundancy removal:

[0128] S108: In order to reduce unnecessary turns in the path and improve the path quality, the original path result Path raw Perform redundancy removal, as shown in the schematic diagram Fig.11 , the process is as follows Fig.12 As shown, the specific steps are as follows:

[0129] Initialize the pruning path node list and replace the original path Path raw The first node in the pruning path is added to the node list.

[0130] Let the last node in the pruning path node list be P start , let P in the original path start The next node is P end .

[0131] P start With P end Collision detection between paths: start With P end Between the two, the search step length Step size Interpolate the interval to get a series of intermediate nodes, then perform collision detection on each intermediate node. If an intermediate node collides, node P end The previous node P end-1 Add to the pruning path node list and add P start Update to P end-1 , return to step b); if all intermediate nodes do not collide, then P end Update to Path rawThe next node in P end Path raw If the last node is found, execute step d), otherwise repeat this step.

[0132] P end Add to the pruning path node list and output the pruning path Path pruned .

[0133] Path Smoothing:

[0134] S109: In order to ensure the smooth operation of the robot arm, the pruning path needs to be smoothly optimized. Since the algorithm proposed in the present invention completes the path planning in Cartesian space, the path smoothing is divided into two parts: position smoothing and posture smoothing. Fig.13 As shown, the specific steps are as follows:

[0135] Initialize the node table.

[0136] Prune the path Path pruned The path points are used as control points, and the quasi-uniform cubic B-spline interpolation is used to obtain the curve SmoothPath with smoothly changing position. RRT* .

[0137] Traverse the pruning path Path pruned For each control point in the smooth curve, find the point closest to the control point and set the posture information of the point on the curve as the posture information of the corresponding control point.

[0138] The smooth path is divided into multiple segments through the smooth path key points with initialized posture information. For each path segment, the posture information of the intermediate path points is obtained by using quaternion spherical linear interpolation based on the posture information of the first and last key points.

[0139] So far, the smoothing process of the improved RRT* algorithm planning path result has been completed. Next, the X obtained in step (1) goal_new With X goal Direct paths are smoothed.

[0140] X-based goal_new With X goal The posture information of the direct path is calculated using quaternion spherical linear interpolation at certain intervals to obtain a direct path SmoothPath with smooth posture. direct .

[0141] SmoothPath RRT* With SmoothPath direct Merge and finally get the connection starting node X start With the original target node X goalA collision-free smooth picking path.

[0142] Robotic arm performs sorting path

[0143] The obtained smooth picking path is converted into a robot arm script language through a host computer, and the robot arm is driven to complete the picking of the target part along the path.

[0144] The present invention has the advantages of fast planning speed, low path cost and few redundant sampling points. In order to prove this point of view, the robot arm material frame sorting path planning method based on improved RRT* fusion artificial potential field proposed in the present invention is used to plan a sorting path for taking parts out of the material basket without collision in the robot arm material frame sorting scene. At the same time, the standard RRT* algorithm, GB-RRT* algorithm (RRT* algorithm with target guided sampling strategy), R-GB-RRT* algorithm (GB-RRT* algorithm with sampling node rejection mechanism) and Informed-RRT* algorithm are used for planning in the same planning scene, which further proves that the present invention is superior to the prior art in the application of robot arm material frame sorting path planning. In order to reduce the impact of algorithm randomness, each algorithm is repeated 100 times, and the average values ​​of algorithm indicators (planning time, path cost, number of sampling points) are statistically compared. The results are shown in Table 1.

[0145]

[0146] Table 1 Comparison of planning indicators of various algorithms

[0147] The above statistical results show that in the same scenario, the robot arm material frame sorting path planning method based on improved RRT* fusion artificial potential field of the present invention reduces the average planning time by 90.83%, 63.15%, 60.28% and 93.60% respectively compared with the standard RRT* algorithm, GB-RRT* algorithm, R-GB-RRT* algorithm and Informed-RRT* algorithm, and reduces the average path cost by 34.75%, 23.64%, 23.64 and 23.91% respectively.

[0148] Fig.14 The original path result and the pruned path result; Fig.15 is the result of the smoothed path. It can be proved that after pruning and smoothing, the number of corners of the path is reduced and the smoothness is optimized. Fig.16 The simulation results of the robot arm picking up along a smooth path prove the effectiveness of the present invention.

[0149] The present invention also proposes a device for intelligent path planning of robot sorting of stacked parts in a basket, comprising the following:

[0150] A data acquisition module is used to acquire the planning scene point cloud through a three-dimensional camera and determine the planning space based on it;

[0151] A first initialization module is used to initialize the starting node and the target node of the planned path;

[0152] An artificial potential field construction module, used to construct an artificial potential field based on the scene point cloud and the target node;

[0153] A path prediction module, used for predicting a first path connecting the starting node and the target node based on the artificial potential field;

[0154] A target node updating module, used for updating the target node based on the predicted first path;

[0155] The second initialization module is used to adaptively initialize the search step size and target guidance probability threshold of the improved RRT* algorithm through the starting position of the target part;

[0156] A path planning module, used to perform path planning from a starting node to an updated target node based on the improved RRT* algorithm to obtain a planning result;

[0157] A path de-redundancy module, used for performing path de-redundancy processing on the planning result to obtain a de-redundant path;

[0158] A path smoothing module, used for performing path smoothing processing on the de-redundant path to obtain a smoothed picking path;

[0159] The robot control module is used to convert the obtained smooth picking path into a robot script language through a host computer, and drive the robot to complete the picking of the target part along the smooth picking path.

[0160] In addition, each functional module in each embodiment of the present invention may be integrated into one processing module, or each module may exist physically separately, or two or more modules may be integrated into one module. The above-mentioned integrated module may be implemented in the form of hardware or in the form of software functional modules.

[0161] If the integrated module is implemented in the form of a software function module and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the present invention implements all or part of the processes in the above-mentioned embodiment method, and can also be completed by instructing the relevant hardware through a computer program. The computer program can be stored in a computer-readable storage medium, and the computer program can implement the steps of the above-mentioned various method embodiments when executed by the processor. Among them, the computer program includes computer program code, and the computer program code can be in source code form, object code form, executable file or some intermediate form. The computer-readable medium may include: any entity or system that can carry the computer program code, recording medium, U disk, mobile hard disk, disk, optical disk, computer memory, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), electrical carrier signal, telecommunication signal and software distribution medium, etc.

[0162] Although the description of the present invention has been quite detailed and specifically describes several described embodiments, it is not intended to be limited to any of these details or embodiments or any particular embodiment, but should be regarded as providing a broad possible interpretation of these claims in view of the prior art by reference to the appended claims, thereby effectively covering the intended scope of the present invention. In addition, the above description of the present invention is based on the embodiments foreseeable by the inventor, and its purpose is to provide a useful description, and those non-substantial changes to the present invention that have not yet been foreseen may still represent equivalent changes to the present invention.

[0163] The above is only a preferred embodiment of the present invention. The present invention is not limited to the above implementation. As long as the technical effect of the present invention is achieved by the same means, it should belong to the protection scope of the present invention. Within the protection scope of the present invention, its technical scheme and / or implementation method can have various modifications and changes.

Claims

1. A method for intelligent sorting path planning of stacked parts in a basket by a robot, characterized in that: Includes the following: The planning scene point cloud is obtained through a 3D camera, and the planning space is determined based on it; Initialize the starting node and target node of the planned path; Constructing an artificial potential field based on the scene point cloud and the target node; Based on the artificial potential field, predicting a first path connecting the starting node and the target node; Based on the predicted first path, updating the target node; Improve the search step size and target guidance probability threshold of the RRT* algorithm through adaptive initialization of the target part's starting position; Based on the improved RRT* algorithm, a path planning is performed from the starting node to the updated target node to obtain a planning result; Performing path de-redundancy processing on the planning result to obtain a de-redundant path; Performing path smoothing processing on the de-redundant path to obtain a smoothed picking path; The obtained smooth picking path is converted into a robot arm script language through a host computer, and the robot arm is driven to complete the picking of the target part along the smooth picking path.

2. The method for intelligently sorting paths of stacked parts in a basket by a robot according to claim 1, characterized in that: Specifically, constructing an artificial potential field based on the scene point cloud and the target node includes: Calculate the radius R of the enclosing sphere of the end of the robot arm, which includes the end-of-arm pickup tool and the currently picked-up part, and As the range of action of the repulsive field P0; first use the formula To construct the gravitational field for the planning space, ξ is the gravitational scale factor, which is set to 0.5, and ρ(q,q goal ) represents the current node q and the target node q goal The distance between them; then through the formula To construct a repulsive field, i.e., an artificial potential field, for the planning space, η represents the repulsive field scale factor, which is set to 5000, and ρ(q,q obs ) represents the obstacle point q obs That is, the distance from the point in the scene point cloud to the current node q.

3. The method for intelligent sorting path planning of stacked parts in a basket by a robot according to claim 2, characterized in that: Specifically, based on the artificial potential field, predicting a first path connecting the start node and the target node includes: Initialization parameters, including the expansion step size Step size , maximum number of iterations Epoch, Initialize the starting node X start With the target node X goal As the starting point and end point of the artificial potential field prediction path, Initialize the first path node list and set the starting node X start Add to the node list, Take the node most recently added to the node list as the current point, and derive the formula of the artificial potential field to obtain the gravity calculation formula And the repulsion calculation formula The gravity calculation formula and repulsion calculation formula can be used to calculate the magnitude of the gravity and repulsion of the current point in the three directions of xyz, and at the same time multiply the resultant force in each direction by the step length Step size The coordinate changes in each direction can be obtained. After superimposing the coordinate changes on the current point, a new node can be obtained and the node is added to the node list. Finally, the distance from the newly added node to the target node is determined. If it is less than the distance threshold, the predicted path is found and the node list is output. Otherwise, this step is repeated until the predicted result is found.

4. The method for intelligently sorting paths of stacked parts in a basket by a robot according to claim 1, characterized in that: Specifically, based on the predicted first path, updating the target node includes: Based on the pose of the starting node and the pose of the target node, the pose corresponding to each path point in the predicted path is calculated by pose linear interpolation; Traverse each path point in the predicted path in turn, connect it directly to the target node, and determine whether the direct path collides, until the path point X corresponding to the path without collision is found. direct , and record the corresponding direct path Path direct ; Update the planned target node to the above path point X direct , denoted as X goal_new .

5. The method for intelligently sorting paths of stacked parts in a basket by a robot according to claim 4, characterized in that: Specifically, the search step size and target guidance probability threshold of the RRT* algorithm are improved by adaptively initializing the starting position of the target part. include, Calculate the angle θ between the Z-axis direction of the starting node and the vertical direction, using the formula Bias = 4.377θ 2 -4.107θ+1Calculate the target guidance probability P of the random tree thresh ; Starting from n=2, the pose corresponding to the first point in the n equally divided points is calculated by linear interpolation of the poses of the starting node and the target node, and the collision of the end of the robot arm in this pose is checked. When the end of the robot arm does not collide with the side wall of the material frame, and the median penetration depth of the end of the robot arm colliding with the remaining parts in the material basket is less than 2mm, record the value of n, calculate the distance dist between the starting node and the target node, and As the search step of the random tree size .

6. The method for intelligently sorting paths of stacked parts in a basket by a robot according to claim 5, characterized in that: Specifically, the path planning from the starting node to the updated target node is performed based on the improved RRT* algorithm to obtain a planning result, including: Step 610: Sample a sampling node X in the planning space based on the target guidance strategy. sample , find the point X on the random tree that is closest to the sampled node nearest , then from X nearest Start and head towards sampling node X sample Direction expansion step Step size , get the new node X new ; Step 620: Determine the obtained node X new Whether the node rejection mechanism is satisfied, if not, return to step 610 to resample, if satisfied, proceed to the next step; Step 630: Check Node X new If the inverse solution result and the collision situation of the node X do not meet the validity requirements, then return to step 610 to resample. Otherwise, select new The random tree node with the lowest path cost after connection is taken as X new The parent node of the new node X new Add to random tree; Step 640: Add the node X newly added to the random tree new As the center, connect the remaining nodes within a certain range with the center point. If the cost of the connected nodes decreases, the center point X new As the parent node of this node; Step 650: Determine whether the random tree has reached the set target node based on the distance between the newly added node and the target node. If it has reached the target node, output the original path Path raw Otherwise, return to step 610 to resample until the random tree is connected to the target node.

7. A method for intelligently sorting paths for stacked parts in a basket by a robot according to claim 6, characterized in that: Specifically, performing path de-redundancy processing on the planning result to obtain a de-redundant path includes: Step 710: Initialize the pruning path result Path pruned , change the original path Path raw The first node P start Add to Path pruned In the example, let P end As Path raw Medium start The next node of Step 720: P start With P end The direct path is used for collision detection; Step 730: Determine the collision detection result. If no collision occurs, set the original path Path raw The next node in P end , execute step 720, otherwise record the node P end In the original path Path raw The previous node P in end-1 , and add it to Path pruned Then the starting node P start Update to P end-1 Then continue to execute step 720, search for path key points backward until the original path Path is traversed raw All key points of the Path pruned .

8. The method for intelligently sorting paths of stacked parts in a basket by a robot according to claim 7, characterized in that: Specifically, performing path smoothing processing on the redundant path to obtain a smoothed picking path includes: Step 810: Remove redundant paths Path pruned The path points are used as control points of the quasi-uniform cubic B-spline, so as to fit a curve with smoothly changing path point positions; Step 820: traverse the control points of the B-spline curve, find the path point closest to the control point in the B-spline curve, and initialize the posture of the path point to the posture of the corresponding control point; Step 830: Divide the smooth curve into multiple segments through the path points with initialized posture information, and use the starting posture and the end posture of each path segment to interpolate the posture corresponding to the intermediate smooth path point, so as to obtain the improved RRT* algorithm path planning result Path after smoothing. smooth ; Step 840: Path according to the collision-free direct path direct , linearly interpolate at certain intervals to obtain the position and posture of the intermediate nodes of the path; Step 850: The Path obtained in step 830 smooth The direct path Path obtained in step 840 direct Merge to generate the final smooth picking path.

9. A device for intelligent path planning of robot sorting of stacked parts in a basket, characterized in that: Includes the following: A data acquisition module is used to acquire the planning scene point cloud through a three-dimensional camera and determine the planning space based on it; A first initialization module is used to initialize the starting node and the target node of the planned path; An artificial potential field construction module, used to construct an artificial potential field based on the scene point cloud and the target node; A path prediction module, used for predicting a first path connecting the starting node and the target node based on the artificial potential field; A target node updating module, used for updating the target node based on the predicted first path; The second initialization module is used to adaptively initialize the search step size and target guidance probability threshold of the improved RRT* algorithm through the starting position of the target part; A path planning module, used to perform path planning from a starting node to an updated target node based on the improved RRT* algorithm to obtain a planning result; A path de-redundancy module, used for performing path de-redundancy processing on the planning result to obtain a de-redundant path; A path smoothing module, used for performing path smoothing processing on the de-redundant path to obtain a smoothed picking path; The robot control module is used to convert the obtained smooth picking path into a robot script language through a host computer, and drive the robot to complete the picking of the target part along the smooth picking path.

Images