A bidirectional RRT-connect industrial robot fast path planning method integrating process knowledge

Abstract

The present invention proposes a bidirectional RRT‑connect algorithm that integrates process knowledge to realize rapid path planning of industrial robots. On the basis of improving the RRT algorithm, the process knowledge is integrated to perform motion planning for the industrial robot. Specifically, the motion planning introduces an expert mechanism to improve the task planning quality of the industrial robot under the premise of integrating process knowledge, and at the same time optimizes the rapid expansion random tree of the target node in combination with the asymptotic optimal idea of ​​the local expansion mechanism, shortens the search time and path cost, and makes the search path converge to the asymptotic optimal solution. The posture of the end of the industrial robot is taken as a node in the working space, and the joint posture is solved by combining inverse kinematics and related process knowledge, and then collision detection is performed using the solved angle. The improved algorithm is integrated with process knowledge to ensure the stability of the planning while realizing a gradually optimized obstacle avoidance path in a short search time, providing technical support for the application of industrial robots in different scenarios.

Description

Technical Field

[0001] The present invention relates to the field of motion planning of process robots, and in particular to a bidirectional RRT-connect industrial robot fast path planning method integrating process knowledge. Background Art

[0002] Industrial robots are widely used in various fields such as industrial production and military. In this field, path planning plays a significant role. At present, there are three common path planning algorithms: direct search algorithms based on the A* algorithm that directly search on the map; heuristic algorithms based on ant colony algorithms and genetic algorithms; algorithms based on sampling strategies such as RRT and PRM. Although the first two strategies can find the global optimal solution, they cannot meet the path planning requirements of complex environments and high-dimensional spaces. In 1988, La Valle proposed the rapidly exploring random tree algorithm (RRT). This method is suitable for robot motion planning problems in high-dimensional spaces. It has probabilistic integrity and fast exploration speed. However, the search of the RRT algorithm is blind and converges slowly. The path generated by random sampling will generate many unnecessary nodes, resulting in an unsmooth and discontinuous path. In addition, due to process constraints, the solution results of the existing path search algorithm are difficult to be directly applied to actual manufacturing production. As the manufacturing industry will be popularized in 2035, digitalization, therefore, a motion planning method for industrial robots based on the improved rapidly expanding tree (RRT) algorithm that integrates process knowledge is proposed. Summary of the invention

[0003] The purpose of this invention is to propose an industrial robot motion planning method based on an improved RRT-connect algorithm that integrates process knowledge. This method is applicable to various industrial sites. At the same time, in view of the shortcomings of the existing technology, RRT is improved in terms of quality and efficiency. The improved algorithm is superior to the traditional RRT algorithm in terms of planning time and various path costs, and the stability of the robot arm is improved.

[0004] To achieve the above object, the present invention proposes a bidirectional RRT-connect industrial robot fast path planning method integrating process knowledge, comprising the following steps:

[0005] S101: Based on process knowledge, determine the feasible solution set of the initial position and target posture of the robot arm, obstacle information, and parameter information of the industrial robot arm;

[0006] S102: Based on the dynamic collision detection algorithm, collision detection is performed on the postures in the start and end point posture set of the robot arm. If there is a collision-free trajectory, the posture with the shortest time is selected as the final start and end posture. If there is no collision-free trajectory, go to S103;

[0007] S103: First, based on the traditional RRT-connect algorithm, the search step size is set as the basis for each step of expansion, and the termination threshold is set as the final connection range of the two random trees at the start and end points; then, new nodes in the search space are quickly screened out based on the expert mechanism;

[0008] S104: The industrial robot starts from the initial position and the end position according to the above strategy, generates two random trees, and then determines whether the two random trees are in a range that can be directly connected according to the termination threshold. If they are lower than the termination threshold, the search is stopped and they are connected. Otherwise, the robot arm continues to execute according to the above strategy until it is lower than the termination threshold.

[0009] S105: Output the position and posture of each path point in order starting from the initial position as a collision-free path.

[0010] Furthermore, the S101 determines the optimal angles (vectors of feature quantities on the Z axis) and second vectors (vectors of feature quantities on the Y axis) of different industrial features based on different types of process specifications for different tools carried by the robot end; for the planned starting position, the collision-free feasible posture W and the corresponding rotation matrix T are solved based on inverse kinematics and collision detection algorithms respectively;

[0011] In order to ensure that the sensor can collect the corresponding data, calculate the angle between the third column of T and the second vector of the process feature (Y axis) ,like If the process angle range is met, the posture is saved; if not, the posture is discarded; finally, the set of starting postures start and goal that meet the specific process requirements are determined.

[0012] Furthermore, in S102, the sets of initial and final postures of the robot arms that meet the process requirements are combined in pairs, and the robot motion trajectory is further obtained by fifth-order polynomial fitting and discretized into multiple groups of configurations. Finally, it is determined whether a conflict occurs between the two postures based on the collision detection algorithm; if there is a conflict-free trajectory, the initial and final postures with the shortest time are selected as the conflict-free trajectory postures, otherwise, go to S103.

[0013] Furthermore, in S103, the sampling points (x, y, z, i, j, k) are searched directly in the Cartesian coordinate system; in order to reduce the search amount of the search space, the sampling points are decomposed into two search spaces - coordinate space (x, y, z) and posture space (i, j, k); and the coordinate space values ​​are searched by random sampling; in order to ensure the effectiveness of the sampling points, random sampling and expert mechanism sampling are integrated to achieve the selection of the posture space; the sampling values ​​of the expert mechanism can be set to: (1, 0, 0), (-1, 0, 0), (0, 1, 0), (0, -1, 0), (0, 0, 1), (0, 0, -1), (( i 1+ i 2) / 2, ( j 1+ j 2) / 2,( k 1+ k 2) / 2),( ,0,0),(0, , 0), (0, 0, ), ( ,0,0),(0, , 0), (0, 0, ), and select the corresponding sampling points according to the probability.

[0014] Furthermore, in the S104, the determined start and goal postures are used as the starting postures for the motion planning of the RRT-connect algorithm. Finally, the bidirectional RRT is used for planning, including the following steps:

[0015] Step 1: Based on the initialization, construct an extended tree tree1 from the starting point to the target point, and construct an extended tree tree2 from the target point to the starting point;

[0016] Step 2: Select a point in the search space and ,if and If it falls in the barrier-free zone, then traverse tree1 and tree2 to find the node closest to the random point. and ;

[0017] Step 3: Judgement and , and Will there be a collision between the lines: If there is no collision between the lines, then judge and , and Is the distance between them less than the expansion step? If and The Euclidean distance between them is less than the expansion step size, then the random point Add to the extended tree; conversely, between the nearest node and the random node Take the point with the extended step length in the direction Add to the expansion tree, then Add to the expansion tree; if and , and If a collision occurs on the line, execute step 2; at the same time, in order to obtain a low-cost initial path, calculate the path and its cost C:

[0018] ,

[0019] When C>Cmax, If rejected, resample and execute step 2;

[0020] Step 4: Determine whether the distance threshold between tree1 and tree2 is less than the termination threshold: If the distance between the two trees is greater than the termination threshold, repeat steps 2 and 3. If the path is smaller than the threshold, the nodes of tree1 and tree2 that meet are traced back to obtain the node sets of the starting point and the target point, and the paths are saved.

[0021] Furthermore, in step 4, a collision detection is performed on the end points of the two random trees tree1 and tree2. If there is no conflict, the two nodes are connected to complete the planning task; if a conflict occurs, the search for a new node continues.

[0022] Compared with the prior art, the advantages of the present invention are: on the basis of improving the RRT algorithm, the present invention integrates process knowledge and performs motion planning for the industrial robot. Specifically, based on the industrial environment in which the industrial robot is located, the corresponding process knowledge is applied to determine the initial posture set of the robot. Based on the initial posture set as a spatial node, dynamic collisions are performed between two of them. At the same time, in the process of bidirectional RRT sampling, an expert mechanism is introduced to enable the industrial robot to find a collision-free path in the workspace while improving the search efficiency and ensuring the path quality. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] Figure 1 Flowchart of industrial robot motion planning based on the improved RRT algorithm integrating process knowledge;

[0024] Figure 2 Schematic diagram of the principle of local expansion mechanism in the improved RRT algorithm. DETAILED DESCRIPTION

[0025] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be further described below.

[0026] The present invention proposes a bidirectional RRT-connect industrial robot fast path planning method integrating process knowledge, such as Figure 1 As shown, the following steps are included:

[0027] S101: Based on the different tools carried by the robot end for different types of process specifications, determine the optimal angle of different industrial features (the vector of the feature quantity on the Z axis) and the second vector (the vector of the feature quantity on the Y axis). For the planned starting position, solve the collision-free feasible pose W and the corresponding rotation matrix T based on inverse kinematics and collision detection algorithms. In order to ensure that the sensor can collect the corresponding data, calculate the angle between the third column of T and the second vector (Y axis) of the process feature ,like If the process angle range is satisfied, the pose is saved. If not, the pose is discarded. Finally, the set of start and goal poses that meet the specific process requirements is determined.

[0028] S102: Based on the dynamic collision detection algorithm, collision detection is performed on each posture in the start and end point posture set of the robot arm. If there is a collision-free trajectory, the posture with the shortest time is selected as the final start and end posture. If there is a conflicting trajectory, go to S103.

[0029] S103: First, based on the traditional RRT-connect algorithm, set a suitable search step length as the basis for each step of expansion, and set a suitable termination threshold as the final connection range of the two random trees at the start and end points. Then, based on the expert mechanism, quickly filter out new nodes in the search space.

[0030] Furthermore, in S103, the sampling points (x, y, z, i, j, k) are searched directly in the Cartesian coordinate system. In order to reduce the search volume of the search space, this paper decomposes the sampling points into two search spaces - coordinate space (x, y, z) and posture space (i, j, k). And the coordinate space values ​​are searched by random sampling; in order to ensure the effectiveness of the sampling points, this paper combines random sampling and expert mechanism sampling to achieve the selection of posture space. The sampling values ​​of the expert mechanism can be set to: (1, 0, 0), (-1, 0, 0), (0, 1, 0), (0, -1, 0), (0, 0, 1), (0, 0, -1), (( i 1+ i 2) / 2, ( j 1+ j 2) / 2,( k 1+ k2) / 2),( ,0,0),(0, , 0), (0, 0, ), ( ,0,0),(0, , 0), (0, 0, ), and select the corresponding sampling points according to the probability.

[0031] S104: The determined start and goal postures are used as the starting postures for the motion planning of the RRT-connect algorithm. Finally, the bidirectional RRT is used for planning, and the steps are as follows:

[0032] 1) If Figure 2 As shown, an extended tree tree1 is constructed from the starting point to the target point, and an extended tree tree2 is constructed from the target point to the starting point;

[0033] 2) According to the sampling method of S101, select points in the search space and ,if and If it falls in the barrier-free zone, then traverse tree1 and tree2 to find the node closest to the random point. and ;

[0034] 3) Judgment and , and Will there be a collision between the lines? If there is no collision between the lines, then and , and Is the distance between them less than the expansion step? If and The Euclidean distance between them is less than the expansion step size, then the random point Add to the expanded tree. Conversely, between the nearest node and the random node Take the point with the extended step length in the direction Add to the expansion tree, then Add to the expansion tree; if and , and If a collision occurs on the line, execute (2); at the same time, in order to obtain a low-cost initial path, calculate the path and its cost C:

[0035] ,

[0036] When C>Cmax, If rejected, re-sample and execute step 2).

[0037] 4) Determine whether the distance threshold between tree1 and tree2 is less than the termination threshold: If the distance between the two trees is greater than the termination threshold, repeat steps 2) and 3). If If the path is smaller than the threshold, the nodes of tree1 and tree2 that meet are traced back to obtain the node sets of the starting point and the target point, and the paths are saved.

[0038] S105: Output the position and posture of each path point in order starting from the initial position as a collision-free path.

[0039] The above is only a preferred embodiment of the present invention and does not limit the present invention in any way. Any technician in the relevant technical field, without departing from the scope of the technical solution of the present invention, makes any form of equivalent replacement or modification to the technical solution and technical content disclosed in the present invention, which does not depart from the content of the technical solution of the present invention and still falls within the protection scope of the present invention.

Claims

1. A bidirectional RRT-connect industrial robot fast path planning method integrating process knowledge, characterized in that: The following steps are involved: S101: Based on process knowledge, determine the feasible solution set of the initial position and target posture of the robot arm, obstacle information, and parameter information of the industrial robot arm; S102: Based on the dynamic collision detection algorithm, collision detection is performed on the postures in the start and end point posture set of the robot arm. If there is a collision-free trajectory, the posture with the shortest time is selected as the final start and end posture. If there is no collision-free trajectory, go to S103; S103: First, based on the traditional RRT-connect algorithm, the search step size is set as the basis for each step of expansion, and the termination threshold is set as the final connection range of the two random trees at the start and end points; then, new nodes in the search space are quickly screened out based on the expert mechanism; In S103, the sampling point (x, y, z, i, j, k) is searched directly in the Cartesian coordinate system; in order to reduce the search amount of the search space, the sampling point is decomposed into two search spaces - coordinate space (x, y, z) and posture space (i, j, k); and the coordinate space value is searched by random sampling; in order to ensure the effectiveness of the sampling point, random sampling and expert mechanism sampling are integrated to realize the selection of the posture space; the sampling value of the expert mechanism can be set to: (1, 0, 0), (-1, 0, 0), (0, 1, 0), (0, -1, 0), (0, 0, 1), (0, 0, -1), (( i 1+ i 2) / 2, ( j 1+ j 2) / 2,( k 1+ k 2) / 2),( ,0,0),(0, , 0), (0, 0, ), ( ,0,0),(0, , 0), (0, 0, ), and select the corresponding sampling points according to the probability; S104: The industrial robot starts from the initial position and the end position according to the above strategy, generates two random trees, and then determines whether the two random trees are in a range that can be directly connected according to the termination threshold. If they are lower than the termination threshold, the search is stopped and they are connected. Otherwise, the robot arm continues to execute according to the above strategy until it is lower than the termination threshold. S105: Output the position and posture of each path point in order starting from the initial position as a collision-free path.

2. The bidirectional RRT-connect industrial robot fast path planning method integrating process knowledge according to claim 1 is characterized in that: The S101 is to determine the best angle (the vector of the feature quantity on the Z axis) and the second vector (the vector of the feature quantity on the Y axis) of different industrial features based on different types of process specifications for different tools carried by the robot end; for the planned starting position, the collision-free feasible posture W and the corresponding rotation matrix T are solved based on inverse kinematics and collision detection algorithms respectively; In order to ensure that the sensor can collect the corresponding data, calculate the angle between the third column of T and the second vector of the process feature (Y axis) ,like If the process angle range is met, the posture is saved; if not, the posture is discarded; finally, the set of starting postures start and goal that meet the specific process requirements are determined.

3. The bidirectional RRT-connect industrial robot fast path planning method integrating process knowledge according to claim 2 is characterized in that: In S102, the sets of initial and final postures of the robot arms that meet the process requirements are combined in pairs, and the robot motion trajectory is further obtained by fitting a fifth-order polynomial and discretized into multiple groups of configurations. Finally, it is determined whether a conflict occurs between the two postures based on the collision detection algorithm; if there is a conflict-free trajectory, the initial and final postures with the shortest time are selected as the conflict-free trajectory postures, otherwise, go to S103.

4. The bidirectional RRT-connect industrial robot fast path planning method integrating process knowledge according to claim 3 is characterized in that: In the S104, the determined start and goal postures are used as the starting postures for the RRT-connect algorithm motion planning. Finally, the bidirectional RRT is used for planning, including the following steps: Step 1: Based on the initialization, construct an extended tree tree1 from the starting point to the target point, and construct an extended tree tree2 from the target point to the starting point; Step 2: Select a point in the search space and ,if and If it falls in the barrier-free zone, then traverse tree1 and tree2 to find the node closest to the random point. and ; Step 3: Judgement and , and Will there be a collision between the lines: If there is no collision between the lines, then judge and , and Is the distance between them less than the expansion step? If and The Euclidean distance between them is less than the expansion step size, then the random point Add to the extended tree; conversely, between the nearest node and the random node Take the point with the extended step length in the direction Add to the expansion tree, then Add to the expansion tree; if and , and If a collision occurs on the line, execute step 2; at the same time, in order to obtain a low-cost initial path, calculate the path and its cost C: ， When C>Cmax, If rejected, resample and execute step 2; Step 4: Determine whether the distance threshold between tree1 and tree2 is less than the termination threshold: If the distance between the two trees is greater than the termination threshold, repeat steps 2 and 3. If the path is smaller than the threshold, the nodes of tree1 and tree2 that meet are traced back to obtain the node sets of the starting point and the target point, and the paths are saved.

5. The bidirectional RRT-connect industrial robot fast path planning method integrating process knowledge according to claim 4 is characterized in that: In step 4, a collision detection is performed on the end points of the two random trees tree1 and tree2. If there is no conflict, the two nodes are connected to complete the planning task; if a conflict occurs, the search for a new node continues.

Images