A robot intelligent sorting path planning method and device for stacking parts in a basket

Abstract

The present application relates to a kind of material basket inner stack part robot intelligent sorting path planning method and device, by combining RRT* algorithm with artificial potential field algorithm, artificial potential field can be used to reduce the random search of RRT* algorithm in free space without barrier, while in the complex obstacle environment close to part pile, using RRT* algorithm carries out accurate collision-free path planning, to avoid the problem of low planning path precision and local minimum of artificial potential field algorithm.It is improved based on this to RRT* algorithm, solve the problem of multiple redundant nodes, low planning efficiency, poor path quality when RRT* algorithm carries out path planning, while, improved RRT* algorithm is combined with artificial potential field algorithm, further reduce the scale of planning, reduce the random search of RRT* algorithm in free space.The method and device proposed in the present application can provide efficient and accurate collision-free picking path planning scheme for industrial applications of mechanical arm material frame sorting.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of mechanical arm path planning, and particularly relates to a robot intelligent sorting path planning method and device for parts stacked in a basket. BACKGROUND

[0002] The purpose of industrial robot path planning is to set the working path of the robot according to the task and the running environment. Nowadays, industrial robots are widely used in target sorting scenes, but in most cases, they are manually taught in a structured environment, which has the defects of simple application scene and weak robot perception ability. When the sorting object faced by the mechanical arm is the scattered and stacked parts in the basket, the path planning of the end of the mechanical arm needs to be completed before sorting. If there is no accurate sorting path planning, the end of the mechanical arm and the target parts will interfere with the basket wall and the remaining parts in the basket during the taking process, even collide, which seriously affects the success rate and safety of sorting. If the manual teaching method is used to set the sorting path, it cannot be applied to different poses of each part in the basket, and lacks practicality and flexibility. Therefore, an accurate and flexible mechanical arm path planning method is needed to meet the industrial application requirements of mechanical arm basket sorting.

[0003] The rapid-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, the algorithm has the characteristics of low computational complexity and good universality, and 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 By introducing parent node reselection and rewiring based on the RRT algorithm, it is guaranteed to search for the optimal solution or a solution close to the optimal solution within a limited time, which improves the path optimization ability of the RRT algorithm and reduces the path cost. However, since the RRT* algorithm randomly samples the state space globally during searching, 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 movement of the robot in the environment by setting the attractive force generated by the target node and the repulsive force generated by the obstacle point. In the case of relatively uniform obstacle layout, the algorithm has good robustness and running rate, but in a complex obstacle environment, the algorithm is easy to fall into a local minimum. SUMMARY

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

[0006] To achieve the above object, the present application adopts the following technical solutions:

[0007] Specifically, a robot intelligent sorting path planning method for stacking parts in a basket is proposed, which includes the following:

[0008] Obtain the planning scene point cloud through a three-dimensional camera, and determine the planning space based on the point cloud;

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

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

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

[0012] Update the target node based on the predicted first path;

[0013] Adaptively initialize the search step and the target guiding probability threshold of the improved RRT* algorithm through the starting pose of the target part;

[0014] Obtain a planning result through path planning from the starting node to the updated target node based on the improved RRT* algorithm;

[0015] Perform path redundancy processing on the planning result to obtain a redundancy-removed path;

[0016] Perform path smoothing processing on the redundancy-removed path to obtain a smoothed picking path;

[0017] Convert the obtained smoothed picking path into a mechanical arm script language through an upper computer to drive the mechanical arm to complete picking of the target part along the smoothed 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 mechanical arm, which includes the end of the mechanical arm picking tool and the current picked part, and set as the action range P0 of the repulsive field; first, use the formula to construct the attractive field of the planning space, where ξ is the attractive scale factor, set to 0.5, and ρ(q,q goal ) represents the distance between the current node q and the target node q goal ; then use the formula to construct the repulsive field of the planning space, i.e., the artificial potential field, where η represents the repulsive field scale factor, set to 5000, and ρ(q,q obs ) represents the obstacle point qobs i.e. the distance of a point in the scene point cloud to the current node q.

[0020] Further, specifically, based on the artificial potential field, a first path connecting the starting node and the target node is predicted, including,

[0021] The initialization parameters include an extended step Step size , and a maximum number of iterations Epoch

[0022] The starting node X start obtained by initialization and the target node X goal are taken as the starting point and the ending point of the artificial potential field prediction path,

[0023] The first path node list is initialized, and the starting node X start is added to the node list,

[0024] The node newly added to the node list is taken as the current point, and the derivative of the artificial potential field formula is used to obtain the gravity calculation formula and the repulsion calculation formula The gravity calculation formula and the repulsion calculation formula can be used to calculate the size of the gravity and the repulsion of the current point in the xyz three directions, and the resultant force in each direction is multiplied by the step Step size , i.e. the coordinate change amount in each direction is obtained, and the new node is obtained after the coordinate change amount is added to the current point, and the node is added to the node list. Finally, the distance from the newly added node to the target node is judged, if it is less than the distance threshold, the prediction path is found, and the node list is output, otherwise the step is repeated until the prediction result is found.

[0025] Further, specifically, based on the first path predicted, the target node is updated, including,

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

[0027] Each path point in the prediction path is sequentially traversed, and it is directly connected with the target node, and it is judged whether the direct connection path collides, until the path point X direct corresponding to the path not colliding is found, and the corresponding direct connection path Path direct is recorded.

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

[0029] Furthermore, specifically, the search step size and target guidance probability threshold of the improved RRT* algorithm are adaptively initialized based on the initial pose of the target part, including:

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

[0031] Starting from n=2, the pose of the first point among the n equally divided points is calculated through linear interpolation of the poses of the starting node and the target node. The collision situation of the robotic arm's end effector in this pose is then checked. If the end effector does not collide with the side wall of the material basket, and the median penetration depth of the parts colliding with other parts in the basket is less than 2mm, the value of n is recorded. The distance dist between the starting node and the target node is calculated, and... Step, the search step size of the random tree size .

[0032] Furthermore, specifically, the path planning result obtained by performing path planning from the starting node to the updated target node based on the improved RRT* algorithm includes:

[0033] Step 610: Based on the target-guided strategy, obtain a sampling node X in the planning space. sample Find the point X on the random tree that is closest to the sampling node. nearest Then from X nearest Depart, towards sampling node X sample Directional expansion step size The new node X is obtained. new ;

[0034] Step 620: Determine the obtained node X new If the node rejection mechanism is not met, return to step 610 to resample; otherwise, proceed to the next step.

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

[0036] Step 640: Using the newly added node X in the random tree newConnect the rest of the nodes within a certain range with the center point, if the cost of the connected nodes decreases, then connect the center point X new to the parent node of the node;

[0037] Step 650: Determine whether the random tree reaches the set target node according to the distance between the newly added node of the random tree and the target node, if the target node is reached, output the original path Path raw , otherwise return to step 610 for resampling until the random tree is connected to the target node.

[0038] Further, specifically, the planning result is subjected to path redundancy removal processing to obtain a redundancy-removed path, including,

[0039] Step 710: Initialize the pruning path result Path pruned , add the first node P raw of the original path Path start to Path pruned , and let P end be the next node of P raw in Path start ;

[0040] Step 720: Perform collision detection on the direct connection path of P start and P end ;

[0041] Step 730: Determine the collision detection result, if no collision occurs, let the next node in the original path Path raw be P end , execute step 720, otherwise record the previous node P end of node P raw in the original path Path end-1 , and add it to Path pruned , then update the starting node P start to P end-1 , then continue to execute step 720 to find the path key point backwardly until all key points of the original path Path raw are traversed, and a redundancy-removed path Path pruned is obtained.

[0042] Further, specifically, the redundancy-removed path is subjected to path smoothing processing to obtain a smoothed pickup path, including,

[0043] Step 810: Take the path points of the redundancy-removed path Path pruned as the control points of quasi-uniform cubic B-spline, so as to fit a curve with smooth variation of path point positions;

[0044] Step 820: traversing the control points of the B-spline curve, finding the path point closest to the control point in the B-spline curve, and initializing the pose of the path point as the pose of the corresponding control point;

[0045] Step 830: dividing the smooth curve into multiple segments by the path points whose pose information has been initialized, and obtaining the pose corresponding to the intermediate smooth path point by interpolating the start pose and the end pose of each segment path, so as to obtain the improved RRT* algorithm path planning result Path smooth after smoothing processing

[0046] Step 840: according to the collision-free direct connection path Path direct , linear interpolation is carried out at a certain interval to obtain the pose of the intermediate node in the path;

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

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

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

[0050] A first initialization module is configured to initialize a start node and a target node of a planning path.

[0051] An artificial potential field construction module is configured to construct an artificial potential field based on the scene point cloud and the target node.

[0052] A path prediction module is configured to predict a first path connecting the start node and the target node based on the artificial potential field.

[0053] A target node updating module is configured to update the target node based on the predicted first path.

[0054] A second initialization module is configured to adaptively initialize a search step length and a target guide probability threshold of an improved RRT* algorithm based on a start pose of a target part.

[0055] A path planning module is configured to perform path planning from the start node to the updated target node based on the improved RRT* algorithm to obtain a planning result.

[0056] A path redundancy removal module is configured to perform path redundancy removal processing on the planning result to obtain a de-redundant path.

[0057] a path smoothing module, configured to perform path smoothing on the de-redundant path to obtain a smoothed picking path;

[0058] a robot arm control module, configured to convert the smoothed picking path into a robot arm script language through a host computer, and drive a robot arm to pick the target part along the smoothed picking path.

[0059] The present application has the following advantages:

[0060] The present application provides a robot intelligent sorting path planning method and device for stacking parts in a basket, 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, the direct connection between the predicted path nodes and the target node is determined, and the effectiveness of the intermediate node pose in the direct connection path is judged, so that the collision-free path from the intermediate point of the predicted path to the target node can be quickly found, the random search of the improved RRT* algorithm in the obstacle-free area is avoided, and the planning efficiency of the path is improved.

[0062] 2. After updating the target node to the intermediate node of the predicted path, the distance between the starting node and the target node is further reduced, the expansion scale of the random tree is reduced, and thus the calculation amount required for planning is reduced, and the speed of searching for a feasible path solution is accelerated.

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

[0064] 4. The improved RRT* algorithm adds an adaptive step length and an adaptive target guidance probability calculation strategy based on the initial pose information of the part, so that the algorithm can automatically calculate a suitable search step length and target guidance probability threshold according to the initial state of the part to adapt to various poses of the part, and improve the stability and adaptability of the algorithm planning.

[0065] 5. The addition of the target guidance sampling strategy enables the random tree to avoid obstacles when expanding and avoid falling into local minimum, and also enables the random tree to quickly approach the target node, achieving a balance between exploration and purpose.

[0066] 6. The introduction of a sampling node rejection mechanism based on the end pose constraint of the robot arm can effectively reduce the number of inverse solution calculations and collision detection times without affecting the planning success rate, thereby reducing the calculation cost 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 motion stability of the robot arm. Attached Figure Description

[0068] The above and other features of this disclosure will become more apparent from the detailed description of the embodiments illustrated in conjunction with the accompanying drawings. In the accompanying drawings, the same reference numerals denote the same or similar elements. Obviously, the drawings described below are merely some embodiments of this disclosure. For those skilled in the art, other drawings can be obtained from these drawings without any creative effort. In the drawings:

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

[0070] Figure 2 This is a schematic diagram of the hardware device platform used in this invention;

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

[0072] Figure 4 A flowchart of a method for intelligent sorting path planning for a robot that stacks parts in a basket;

[0073] Figure 5 This is a schematic diagram illustrating how to predict sorting paths and update target nodes using an artificial potential field.

[0074] Figure 6 A schematic diagram illustrating the planning using the improved RRT* algorithm after updating the target node;

[0075] Figure 7 This is a sub-flowchart for step S104;

[0076] Figure 8 This is a sub-flowchart for step S105;

[0077] Figure 9 This is a sub-flowchart for step S106;

[0078] Figure 10 This is a sub-flowchart for step S107;

[0079] Figure 11 This is a schematic diagram of step S108;

[0080] Figure 12 This is a sub-flowchart for step S108;

[0081] Figure 13 This is a sub-flowchart for step S109;

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

[0083] Figure 15An example of path smoothing results in one embodiment

[0084] Figure 16 An example of simulation results for a robot arm. DETAILED DESCRIPTION

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

[0086] In view of the problems existing in the RRT* algorithm, in combination with the industrial application scene of robot picking of parts stacked in a basket, the embodiment 1, with reference to Figure 1 , the present application proposes a robot intelligent picking path planning method for parts stacked in a basket, comprising the following steps:

[0087] Obtain the planning scene point cloud through a three-dimensional camera, and determine the planning space based on the point cloud;

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

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

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

[0091] Update the target node based on the predicted first path;

[0092] Adaptively initialize the search step and the target guiding probability threshold of the improved RRT* algorithm from the starting pose of the target part;

[0093] Obtain a planning result by performing path planning from the starting node to the updated target node based on the improved RRT* algorithm;

[0094] Perform path redundancy processing on the planning result to obtain a redundancy-removed path;

[0095] Perform path smoothing processing on the redundancy-removed path to obtain a smoothed picking path;

[0096] Convert the obtained smoothed picking path into a robot script language through the upper computer, and drive the robot arm to complete picking of the target part along the smoothed picking path.

[0097] In the hardware part, a computer is used as an upper computer, and data exchange and control are performed with the three-dimensional camera and the robot arm through a network switch. The hardware device schematic diagram is shown in Figure 2 ;

[0098] In this embodiment 1, by combining the RRT* algorithm with the artificial potential field algorithm, the artificial potential field can reduce the random search of the RRT* algorithm in unobstructed free space. Simultaneously, in complex obstacle environments near the parts stack, the RRT* algorithm is used for accurate collision-free path planning, thus avoiding the low path planning accuracy and getting trapped in local minima problems of the artificial potential field algorithm. Based on this, the present invention provides a method and apparatus for intelligent sorting path planning for a robot stacking parts in a basket. In the algorithm part, the RRT* algorithm is improved to solve the problems of redundant nodes, low planning efficiency, and poor path quality in path planning. Furthermore, combining the improved RRT* algorithm with the artificial potential field algorithm further reduces the planning scale and decreases the random search in free space by the RRT* algorithm. In the hardware part, a computer acts as the host computer, exchanging and controlling data with a 3D camera and a robotic arm through a network switch. The 3D camera is used as a scene perception tool to acquire the point cloud of the object to be picked up. The sorting path is planned using the aforementioned algorithm, and the host computer drives the robotic arm to pick up the object. Based on this method and device, an efficient and accurate collision-free picking path planning scheme is provided for industrial applications of robotic arm material box sorting.

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

[0100] Planning initialization steps:

[0101] S101: Acquire scene point clouds by taking pictures with a 3D camera, extract box point clouds in the scene, and construct planning space based on the bounding box size of the box point clouds.

[0102] S102: Obtain the placement pose of the target part to be picked up in the material box through a pose estimation algorithm, and initialize it as the planned starting node X. start The robotic arm's state upon completion of the pickup is set through manual teaching, and the pose of the target part corresponding to that state is initialized to the target node X. goal .

[0103] S103: Calculate the radius R of the bounding sphere of the robotic arm end effector (including the robotic arm end effector picking tool and the currently picked part), and then... The range of action of the repulsive field is P0; firstly, using the formula... To construct a gravitational field in the planned space, where ξ is the gravitational scale factor, set to 0.5, ρ(q,q goal ) represents the current node q and the target node q goal The distance between them; then expressed by the formula repulsive force field is constructed for the planning space, where η represents a repulsive force field scale factor, set to 5000, and ρ(q, q obs ) represents the distance from the obstacle point q obs (i.e., the point in the point cloud obtained by scanning) to the current node q.

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

[0105] The parameters are initialized, including the extended step length Step size (set to 5) and the maximum number of iterations Epoch (set to 15000).

[0106] The start node and the target node are initialized, and the start node X start and the target node X goal obtained in step S102 are taken as the starting point and the end point of the artificial potential field prediction path.

[0107] The prediction path node list is initialized, and the start node is added to the node list.

[0108] The node newly added to the node list is taken as the current point, and the derivative of the formula of the artificial potential field in S103 is taken to obtain the gravity calculation formula and the repulsive force calculation formula Using the formula, the magnitude of the gravity and the repulsive force in the xyz three directions of the current point can be calculated, and the resultant force in each direction is multiplied by the step length Step size , i.e., the coordinate change amount in each direction is obtained. After the coordinate change amount is added to the current point, a new node is obtained, and the node is added to the node list. Finally, the distance from the newly added node to the target node is judged. If it is less than the threshold value, the prediction path is found, and the node list is output. Otherwise, the step is repeated until the prediction result is found.

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

[0110] Based on the pose information of the start node X start and the target node X goal , the pose corresponding to each prediction path point is obtained by linear interpolation.

[0111] The start node of the prediction path is taken as the current checkpoint X, and the node is taken as the target node X goalDirectly connecting, calculating the length of the direct connection path, linearly interpolating a series of intermediate nodes X between the current checkpoint and the target node at intervals of 5 inter , updating the pose of the end of the robot arm (including the picking mechanism and the target part) using the pose of the intermediate node, and determining whether the end does not collide in each pose condition, if not, the current checkpoint X is taken as the new target node X goal_new , otherwise the next node of the predicted path is taken as the current checkpoint, and the step is repeated until the checkpoint that meets the condition is found.

[0112] As shown in Figure 6 , after completing the above planning initialization step, a new target node X goal_new is found by artificial potential field without collision direct connection path to the original target node X goal , and then the path planning from the starting node X start to the new target node X goal_new is completed based on the improved RRT* algorithm.

[0113] Random tree search step:

[0114] S106: adaptively initialize the search step and the target guidance probability threshold of the improved RRT* algorithm from the target part starting pose, as shown in Figure 9 , the specific steps are as follows:

[0115] Calculate the size of 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 (calculated by points (0, 1), points points ).

[0117] From n = 2, the pose corresponding to the first point in the n equal division points is calculated by linearly interpolating the pose of the starting node X start and the current target node X goal_new , and the collision condition of the end of the robot arm (including the end picking tool and the current picking target part) in this pose is checked. When the robot arm end does not collide with the side wall of the material frame, and the median penetration depth of the end with the remaining parts in the material basket is less than 2mm, the value of n is recorded, the distance dist between the starting node and the target node is calculated, and is taken as the search step Step size of the random tree, if the above collision condition is not met, let n = n + 1 and repeat the step.

[0118] S107: After determining the search step and the target guidance probability, start path planning from the starting node to the current target node based on the improved RRT* algorithm, and the flow is as shown in Figure 10 The specific steps are as follows:

[0119] Randomly take a number P between [0, 1], and judge the size relationship between P and Bias. When P is greater than Bias, the algorithm directly takes the target node X goal_new as the sampling node X sample , otherwise, a point X rand is randomly sampled in the planning space as X sample .

[0120] According to the sampling point X sample , find the nearest point X sample to X nearest in the random tree, and expand Step nearest from X sample to X size to get the node X new .

[0121] Calculate the angle a between the pose normal vector corresponding to the node X new and the vertical direction vector, and compare it with , where represents the angle between the pose normal direction corresponding to the starting node and the vertical direction. If a is greater than , the node X new is considered invalid, and the previous step is returned to resample, otherwise, the node is valid, and the next step is performed.

[0122] Inverse solution check: establish the kinematic model of the robot arm by the Modified-DH parameter method, and perform inverse solution calculation on the new node X new by the analytical method. When at least one set of joint angles in the inverse solution satisfies the joint travel limit, the next collision detection is performed, otherwise, return to step a) to resample.

[0123] Collision detection: construct the BVH model in the FCL library according to the STL format model of the robot arm end and the target object, divide the object into the smallest primitive by the BVH model and generate the collision object, update the pose of the collision object according to the node X new , detect whether the robot arm end tool and the picked object under the pose collide with the remaining objects in the space, if 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 is connected to the node, and X newThe corresponding path has the lowest cost, and this tree node is used as the current sampling node X. new The parent node, thus making the new node X new Add it to the random tree.

[0125] Rewiring: Using the newly added node X in the random tree new Centered on a given node, connect each of the random tree nodes within a certain range to that node. Determine if the path cost of the connected random tree nodes is reduced. If it is reduced, update the parent node of that tree node to node X. new .

[0126] Determine the node X newly added to the random tree new If the distance to the target node is less than a certain threshold, the random tree is considered to have expanded to the target node. In this case, node backtracking is performed to generate the path result. raw Otherwise, return to step a) to continue the search.

[0127] Path redundancy removal:

[0128] S108: To reduce unnecessary turns in the path and improve path quality, the original path result is modified. raw Redundancy removal is performed, as shown in the diagram. Figure 11 The process is as follows Figure 12 As shown, the specific steps are as follows:

[0129] Initialize the list of pruned path nodes, including the original path. raw The first node is added to the list of pruning path nodes.

[0130] Let the last node in the list of pruning path nodes be P. start Let P in the original path start The next node as P end .

[0131] For P start With P end Collision detection is performed on the paths between P: start With P end Between the two, the search step size is Step size Interpolation is performed at intervals to obtain a series of intermediate nodes. Then, collision detection is performed on each intermediate node. If a collision occurs between intermediate nodes, node P is removed. end The previous node P end-1 Add it to the list of pruning path nodes, and set P start Updated to P end-1 Return to step b); if no collision occurs among all intermediate nodes, then set P... end Update to Path rawthe next node in P end if P raw is the last node in Path end , execute step d), otherwise repeat this step.

[0132] add P end to the pruned path node list, output the pruned path Path pruned .

[0133] Path smoothing:

[0134] S109: In order to run smoothly, the pruned path needs to be smoothed and optimized. Since the algorithm proposed in the present application completes path planning in Cartesian space, the smoothing of the path is divided into two parts: position smoothing and attitude smoothing. The flow is shown in Figure 13 , and the specific steps are as follows:

[0135] Initialize the node table.

[0136] use the path points of the pruned path Path pruned as control points, and use quasi-uniform cubic B-spline interpolation to obtain a curve SmoothPath RRT* with smooth position changes.

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

[0138] Divide the smooth path into multiple segments through the key points with initialized attitude information. For each segment of path, use quaternion spherical linear interpolation based on the attitude information of the first and last key points to obtain the attitude information of the intermediate path points.

[0139] By now, the smoothing of the path planning result of the improved RRT* algorithm has been completed. Next, the straight connection path between X goal_new and X goal obtained in step (i) is subjected to attitude smoothing.

[0140] Based on the attitude information of X goal_new and X goal , use quaternion spherical linear interpolation to calculate the attitude of the intermediate nodes in the straight connection path at certain intervals, thereby obtaining the straight connection path SmoothPath direct with smoothed attitude.

[0141] merge SmoothPath RRT* and SmoothPath direct , and finally obtain the straight connection path between the starting node X start and the original target node X goalA collision-free smooth pick-up path.

[0142] Mechanical arm executes a sorting path

[0143] The obtained smooth pick-up path is converted into a mechanical arm script language by a host computer to drive the mechanical arm to complete picking up the target part along the path.

[0144] The present application has the advantages of fast planning speed, low path cost and fewer redundant sampling points. To prove this point, the improved RRT* based on artificial potential field for mechanical arm pallet sorting path planning method is used to plan a collision-free picking path for parts from the pallet in the mechanical arm pallet 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 to plan in the same planning scene to further prove that the present application is superior to the prior art in the application of mechanical arm pallet sorting path planning. To reduce the influence of algorithm randomness, each algorithm is repeated 100 times, and the average values of the algorithm indicators (planning time, path cost, sampling point number) are compared, and the results are shown in Table 1.

[0145]

[0146] Table 1 Comparison of planning indicators of each algorithm

[0147] From the above statistical results, in the same scene, the improved RRT* based on artificial potential field for mechanical arm pallet sorting path planning method of the present application reduces the average planning time by 90.83%, 63.15%, 60.28% and 93.60% compared with the standard RRT* algorithm, GB-RRT* algorithm, R-GB-RRT* algorithm and Informed-RRT* algorithm, respectively, and reduces the average path cost by 34.75%, 23.64%, 23.64 and 23.91%, respectively.

[0148] Figure 14 The original path result and the pruned path result; Figure 15 The smooth path result. It can be proved that after pruning and smoothing, the number of corners of the path is reduced and the smoothness is optimized. Figure 16 The simulation result of the mechanical arm picking along the smooth path proves the effectiveness of the present application.

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

[0150] The data acquisition module is configured to acquire a planning scene point cloud by using a three-dimensional camera, and determine a planning space based on the planning scene point cloud.

[0151] The first initialization module is configured to initialize a start node and a target node of a planning path.

[0152] The artificial potential field construction module is configured to construct an artificial potential field based on the scene point cloud and the target node.

[0153] The path prediction module is configured to predict a first path connecting the start node and the target node based on the artificial potential field.

[0154] The target node updating module is configured to update the target node based on the predicted first path.

[0155] The second initialization module is configured to adaptively initialize a search step length and a target guiding probability threshold of the improved RRT* algorithm based on a target part start pose.

[0156] The path planning module is configured to perform path planning from the start node to the updated target node based on the improved RRT* algorithm to obtain a planning result.

[0157] The path redundancy removal module is configured to perform path redundancy removal processing on the planning result to obtain a redundancy-removed path.

[0158] The path smoothing module is configured to perform path smoothing processing on the redundancy-removed path to obtain a smoothed picking path.

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

[0160] In addition, each functional module in each embodiment of the present application can be integrated in one processing module, or each module can exist physically independently, or two or more modules can be integrated in one module. The integrated module can be realized in the form of hardware or in the form of a software functional module.

[0161] The integrated module, if implemented in the form of a software function module and sold or used as an independent product, can be stored in a computer readable storage medium. Based on such understanding, all or part of the processes in the above-mentioned embodiment methods can also be completed by a computer program instructing related hardware, and the computer program can be stored in a computer readable storage medium. When the computer program is executed by a processor, the steps of the above-mentioned various method embodiments can be implemented. The computer program includes computer program code, which can be in the form of source code, object code, executable files or some intermediate forms. The computer readable medium can include any entity or system capable of carrying the computer program code, recording medium, U disk, mobile hard disk, magnetic disk, optical disk, computer memory, read-only memory (ROM), random access memory (RAM), electrical carrier signal, telecommunication signal and software distribution medium, etc.

[0162] Although the description of the present application has been quite detailed and particularly described with respect to several described embodiments, it is not intended to be limited to any of these details or embodiments or any particular embodiment, but should be considered to be a broad interpretation of the claims in view of the prior art, so as to effectively encompass the intended scope of the present application. In addition, the present application is described above in embodiments that the inventors can foresee, and the purpose is to provide a useful description, and those non-essential modifications to the present application that have not yet been foreseen can still represent equivalent modifications of the present application.

[0163] The above is only the preferred embodiment of the present application, and the present application is not limited to the above-mentioned embodiments, as long as the same means achieve the technical effect of the present application, which should belong to the protection scope of the present application. The technical solutions and / or embodiments within the protection scope of the present application can have various modifications and changes.

Claims

1. A robot intelligent sorting path planning method for stacking parts in a basket, characterized in that, The method comprises the following steps: acquiring a scene point cloud of a planning area by a three-dimensional camera, and determining a planning space based on the scene point cloud; initializing a start node and a target node of a planning path; constructing a potential field based on the scene point cloud and the target node; predicting a first path connecting the start node and the target node based on the potential field; updating the target node based on the predicted first path; adaptingly initializing a search step length and a target guiding probability threshold of an improved RRT* algorithm through a target part start pose; planning a path from the start node to the updated target node based on the improved RRT* algorithm to obtain a planning result; performing path redundancy removal processing on the planning result to obtain a redundancy-removed path; performing path smoothing processing on the redundancy-removed path to obtain a smoothed picking path; converting the smoothed picking path into a mechanical arm script language through an upper computer to drive the mechanical arm to complete picking of the target part along the smoothed picking path. Specifically, updating the target node based on the predicted first path comprises: calculating a pose corresponding to each path point in the predicted path through linear interpolation of the start node pose and the target node pose. Traverse each path point in the prediction path in sequence, connect it with the target node directly, judge whether the direct connection path collides or not, until the path point corresponding to the path not colliding is found , and the corresponding direct connection path is recorded ; updating the planned target node to the path point , denoted as ; Specifically, adaptingly initializing the search step length and the target guiding probability threshold of the improved RRT* algorithm through the target part start pose comprises: Calculate the included angle between the Z-axis direction of the start node and the vertical direction , by formula Calculate the target guide probability of the random tree ; From Start, the first point in the n equal point is calculated by the linear interpolation of the pose of the starting node and the target node, and the collision of the mechanical arm end is checked under the attitude, when the mechanical arm end does not collide with the side wall of the material frame, and the median of the penetration depth of the collision part between the end and the remaining parts in the material basket is less than 2mm, the value of is recorded, the distance between the starting node and the target node is calculated, and is taken as the search step of the random tree ; Specifically, planning the path from the start node to the updated target node based on the improved RRT* algorithm to obtain the planning result comprises: Step 610: Based on the target-guided strategy, sample a node in the planning space. Find the point on the random tree that is closest to the sampling node. Then from Depart, towards the sampling node Directional expansion step size Get a new node ; Step 620: Determine the obtained nodes If the node rejection mechanism is not met, return to step 610 to resample; otherwise, proceed to the next step. Step 630: Check the nodes If the inverse solution result and collision situation do not meet the validity requirements, return to step 610 to resample; otherwise, select the node. The random tree node with the lowest path cost after connection is used as The parent node, thus the new node Add to the random tree; Step 640: with the newly added node to the random tree As the center, connect the remaining nodes within a certain range with the center point, if the cost of the connected node decreases, then connect the center point As the parent node of the node; Step 650: judging whether the random tree reaches the set target node according to the distance between the newly added node of the random tree and the target node, and outputting the original path if the target node is reached , otherwise returning to step 610 for resampling until the random tree is connected to the target node.

2. The method of claim 1, wherein, Specifically, constructing the potential field based on the scene point cloud and the target node comprises: Computing a surrounding sphere radius of a robot end effector , the robot end effector including a robot end effector pick tool and a current pick part, and an action range of a repulsive field ; first constructing a gravitational field for a planning space by using a formula , wherein is a gravitational scale factor, set as 0.5, represents a distance between a current node and a target node ; subsequently constructing a repulsive field, i.e. artificial potential field, for the planning space by using a formula , wherein represents a repulsive field scale factor, set as 5000, represents a distance from an obstacle point , i.e. a point in a scene point cloud, to the current node .

3. The method of claim 2, wherein, Specifically, predicting the first path connecting the start node and the target node based on the potential field comprises: Initialization parameters, including extended step size , maximum number of iterations , The starting node obtained by the initialization and the target node as the start and end points of the artificial potential field predicted path, initialize a first path node list and add the start node to the node list, The node newly added in the node list is taken as a current point, and a gravity calculation formula and a repulsion calculation formula are obtained by derivation of the artificial potential field formula and the repulsion calculation formula The gravity and repulsion in three directions of the current point can be calculated by using the gravity calculation formula and the repulsion calculation formula, and the coordinate change amount in each direction can be obtained by multiplying the resultant force in each direction by a step length The new node can be obtained by adding the coordinate change amount to the current point, the node is added to the node list, and the distance from the newly added node to the target node is determined. If the distance is less than a distance threshold, the prediction path is found, the node list is output, otherwise, the step is repeated until the prediction result is found.

4. The method of claim 1, wherein, Specifically, performing the path redundancy removal processing on the planning result to obtain the redundancy-removed path comprises: Step 710: Initialize the pruning path results , the original path The first node Join In the middle, order As middle The next node; Step 720: performing collision detection on the direct path of with the direct path of Step 730: Determine the collision detection result. If no collision occurs, then let the original path... The next node in the process is as Execute step 720; otherwise, record the node. In the original path The previous node in and add it to In the middle, then the starting node Updated to Then continue with step 720, searching for key points along the path until the original path has been traversed. All key points are used to obtain the redundancy removal path. .

5. The method of claim 4, wherein, Specifically, performing the path smoothing processing on the redundancy-removed path to obtain the smoothed picking path comprises: Step 810: Remove redundancy paths The path points are used as control points for quasi-uniform cubic B-splines, thereby fitting a curve with a smooth change in the position of the path points. Step 820: traversing the control points of the B-spline curve, finding the path point closest to the control point in the B-spline curve, and initializing the pose of the path point as the pose of the corresponding control point; Step 830: divide the smooth curve into multiple segments by the path points of which the initialized pose information is initialized, and interpolate the pose corresponding to the intermediate smooth path point by using the start pose and the end pose of each segment path, so as to obtain the improved RRT* algorithm path planning result after the smooth processing ; Step 840: obtaining the pose of the intermediate node in the collision-free direct connection path according to the interval linear interpolation ; Step 850: Take the result from step 830 The direct connection path obtained in step 840 Merge to generate the final smooth pick path.

6. A device for intelligent sorting path planning of stacked parts in a basket by a robot, characterized in that, The device comprises the steps of the method of any one of claims 1-5, The following: a data acquisition module configured to acquire a scene point cloud of a planning area by a three-dimensional camera, and determine a planning space based on the scene point cloud; a first initialization module configured to initialize a start node and a target node of a planning path; a potential field construction module configured to construct a potential field based on the scene point cloud and the target node; a path prediction module configured to predict a first path connecting the start node and the target node based on the potential field; a target node updating module configured to update the target node based on the predicted first path; a second initialization module configured to adaptively initialize a search step length and a target guiding probability threshold of an improved RRT* algorithm through a target part start pose; a path planning module configured to plan a path from the start node to the updated target node based on the improved RRT* algorithm to obtain a planning result; a path redundancy removal module configured to perform path redundancy removal processing on the planning result to obtain a redundancy-removed path; and a path smoothing module configured to perform path smoothing processing on the redundancy-removed path to obtain a smoothed picking path. a path smoothing module, configured to perform path smoothing on the de-redundant path to obtain a smoothed picking path; a mechanical arm control module, configured to convert the smoothed picking path into a mechanical arm script language through a host computer, and drive a mechanical arm to complete picking of the target part along the smoothed picking path.

Images