Three-dimensional path planning method for micro-assembly collaborative robot

Abstract

The invention provides a three-dimensional path planning method for a micro-assembly collaborative robot, and the method comprises the following steps: S1, system initialization: determining the position coordinates of a path starting point and an end point, the step length of each connection, the maximum iteration time or frequency, and an initial search range; s2, carrying out sampling search on an initial path; s3, carrying out improved Inform-RRT * algorithm sampling, wherein the improved Inform-RRT * algorithm sampling comprises a self-adaptive oblique cylinder region limiting strategy in a three-dimensional space and a path segmentation limiting sampling strategy that a path is segmented into different parts by sharp turns; s4, if the optimal path is searched and the upper limit of the search time or the number of iterations is reached, outputting the current optimal path; through the improved Inform-RRT * algorithm, the collaborative robot can perform optimal path planning in a three-dimensional complex environment, the quality of sampling points can be improved, and the convergence speed of the traditional Inform-RRT * algorithm to the optimal path under the condition of complex environment is accelerated.

Description

Technical Field

[0001] The present invention relates to the technical field of path planning, and in particular to a three-dimensional path planning method for a micro-assembly collaborative robot. Background Art

[0002] The micro-assembly system can complete the assembly of tiny parts through the precise coordination between various subsystems. The main role of collaborative robots in assembly tasks is loading and unloading and auxiliary assembly. Before this process, the path planning of collaborative robots is required. Collaborative robot path planning refers to planning a collision-free path from the starting point to the target point based on the structural parameters of the collaborative robot itself, environmental information, the starting point it needs to start from, and the target point it needs to reach.

[0003] The RRT algorithm is a popular three-dimensional path planning method. Its main idea is to perform random sampling in a limited search space and screen the sampling points until a collision-free path is found. The RRT* algorithm has asymptotic optimality. It adds a path optimization process to the original RRT. It does not stop sampling after finding a path. In subsequent sampling, it updates the parent-child relationship in the tree through the ChooseParent and Rewire processes until a certain requirement is met and stops sampling and outputs the optimal path.

[0004] The Informed-RRT* algorithm optimizes the situation where the RRT* algorithm converges slowly to the optimal solution. It can limit the sampling range of the RRT* algorithm after obtaining the initial path, and update the sampling range according to the current optimal path to improve the quality of the sampling points, thereby accelerating the convergence of the algorithm to the optimal solution.

[0005] However, the Informed-RRT* algorithm is currently mainly used in two-dimensional planes. In the path planning application of three-dimensional plane micro-assembly collaborative robots, since the working path of the collaborative robot generally needs to bypass large obstacles, the path is divided into different segments by some sharp turns. In this case, there is currently no path planning method that can effectively limit the sampling range and thus accelerate the convergence of the algorithm. Summary of the invention

[0006] The purpose of the present invention is to solve the shortcomings of the above-mentioned existing path planning algorithms and propose a three-dimensional path planning method for a micro-assembly collaborative robot, which can effectively limit the sampling range, has high sampling quality, and accelerates the convergence of the algorithm.

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

[0008] A three-dimensional path planning method for a micro-assembly collaborative robot specifically includes the following steps: S1, system initialization, which includes the initialization of collaborative robot information, environmental information, the initialization of the collaborative robot's starting point and end point, and the initial search range; S2, obtaining the initial path through the RRT* algorithm; S3, based on the improved Informed-RRT* algorithm, an adaptive oblique cylinder area restriction strategy in three-dimensional space and a path segment restriction sampling strategy for paths divided into different parts by sharp bends are introduced. After obtaining the initial solution, the oblique cylinder area sampling restriction is performed according to the planned optimal path to improve the quality of the sampling points and accelerate the convergence to the optimal solution; S4, when the optimal path is planned, or the set iteration time or number limit is reached, the path planning is stopped and the shortest path at that time is output as the planned path. S5, the path point position information in the path is transmitted to the collaborative robot, and the collaborative robot obtains the angle of each joint through the inverse solution of the position information to complete the motion planning under the path.

[0009] Furthermore, the system initialization in S1 also includes initialization of each iteration step, end point range judgment, maximum number of iterations or iteration time, initial sampling range, and path segment number n.

[0010] Further, S3 specifically includes the following steps: S3.1, for the adaptive oblique cylinder area restriction strategy in three-dimensional space and the path segment restriction sampling strategy for the path divided into different parts by sharp turns, the current optimal path should be divided into n segments according to the set path segment number n, and an integer m is randomly selected between 1 and n, and two segment combinations of path restrictions are selected according to the integer and the specified segment selection rules; S3.2, points in two paths are randomly selected in the two segment combinations, and the path segment contained between the two points is used as The reference of the restricted range of the oblique ellipsoid is that the two points are respectively used as the starting point and the end point of the segment, and the straight line passing through the two points is used as the median axis of the oblique cylinder; S3.3, solve the absolute value of the coordinate system difference between the starting point and the end point of the path segment, the upper and lower bases of the oblique cylinder are perpendicular to the coordinate system corresponding to the maximum absolute value of the difference, and the maximum and minimum values ​​of the path in this coordinate system are used as the positions of the upper and lower bases of the oblique cylinder; S3.4, calculate the distances of the points contained in the segment in the two-dimensional plane parallel to the upper and lower bases of the oblique cylinder and the median axis in turn, and select the maximum value of the distance as the radius r of the oblique cylinder max , construct the oblique cylinder sampling restriction area. max The solution formula is:

[0011] r max =argmax{ρ(σ(x))|σ(x0)=x1,σ(x num-1 )=x2}

[0012] Among them, σ represents the intercepted path segment, num represents the number of path points contained in the path segment, x represents the path point, and x1 and x2 are the starting point and end point of the corresponding selected path segment. S3.5. Random sampling and path optimization are performed in the sampling restriction area of ​​the oblique cylinder.

[0013] Furthermore, the method corresponding to path optimization according to sampling points in S3 is the path optimization method of the RRT* algorithm, which includes the path optimization processes of ChooseParent and Rewire.

[0014] Further, the corresponding parameters in S3.1 are selected; specifically, the following steps are included: S3.11, determining the number of segmentation segments n according to the general direction of the path, and adding 1 to the number of segmentation segments of the path due to sharp turns; S3.12, selecting the path segment according to the value of m and the specified segment selection rule, wherein the segment selection rule is as follows: Let x start is the starting point of the currently planned optimal path, x goal is the end point of the currently planned optimal path, c min is the path length of the currently planned optimal path, x1 and x2 are randomly selected path points in the path segment, corresponding to the starting point and end point of the selected path segment. When m = 1, x1 = x start , from [c min / n,2*c min / n] randomly selects a path point as x2; when m = 2, from [c min / n,2*c min / n] randomly selects a path point as x1 from the path segment of [2*c min / n,3*c min / n] randomly selects a path point as x2; and so on, until when m=n-1, select [(n-2)*c min / n,(n-1)*c min / n] randomly selects a path point as x1, x2 = x goal ; When m = n, let x1 = x start , x2=x goal , in order to ensure the probabilistic completeness of the algorithm.

[0015] Furthermore, S3.5 performs random sampling in the sampling restriction area of ​​the oblique cylinder; specifically includes the following steps: S3.51, randomly select a point within the range of the upper and lower bases of the oblique cylinder on the coordinate system axis perpendicular to the upper and lower bases of the oblique cylinder, and the coordinate value corresponding to the point on the axis is used as a coordinate value corresponding to the sampling point. S3.52, randomly select a point within the range of the upper and lower bases of the oblique cylinder on the coordinate system axis perpendicular to the upper and lower bases of the oblique cylinder, and use the coordinate value corresponding to the point on the axis as a coordinate value corresponding to the sampling point. maxS3.53 randomly selects a value of angle between 0 and 360 degrees; S3.54, in a plane parallel to the upper and lower bases of the oblique cylinder and passing through the random point in S3.51, draw a circle with the intersection of the median axis of the oblique cylinder and the plane as the center and r as the radius. The point on the circle corresponding to angle is the sampling point.

[0016] Furthermore, in the sampling points corresponding to angle angle in S3.54, the reference 0 degree corresponding to angle angle can be a random position or a fixed position, both of which can meet the requirements of random sampling.

[0017] In general, the present invention has the following advantages:

[0018] 1. The present invention improves the Informed-RRT* algorithm to make it more suitable for the three-dimensional path planning of micro-assembly collaborative robots, so that the collaborative robots can plan an asymptotically optimal collision-free path in different three-dimensional complex environments.

[0019] 2. The present invention introduces an adaptive oblique cylinder area restriction strategy in three-dimensional space into the Informed-RRT* algorithm, which effectively solves the problem of poor sampling range limitation in three-dimensional path planning of collaborative robots, improves the quality of sampling points, and accelerates the convergence of the algorithm to the optimal solution in three-dimensional path planning.

[0020] 3. The present invention introduces a path segmentation restriction sampling strategy into the above-mentioned improved Informed-RRT* algorithm for the path divided into different parts by sharp turns, thereby effectively solving the problem that the sampling range cannot be well limited in the path of the collaborative robot bypassing large obstacles, and improving the quality of the sampling points without losing the probability completeness. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] Figure 1 It is a schematic diagram of the overall process of the three-dimensional path planning method of the micro-assembly collaborative robot based on the improved Informed-RRT* algorithm of the present invention;

[0022] Figure 2 Flowchart of the sampling strategy for improving the path segmentation restriction in the Informed-RRT* algorithm for paths divided into different parts by sharp bends;

[0023] Figure 3 Schematic diagram of the oblique cylinder construction process to improve the adaptive oblique cylinder region restriction strategy in three-dimensional space in the Informed-RRT* algorithm;

[0024] Figure 4 Schematic diagram of the random sampling process in the oblique cylinder of the improved Informed-RRT* algorithm;

[0025] Figure 5 It is a schematic diagram of the region restriction strategy of the oblique cylinder in three-dimensional space;

[0026] Figure 6 Schematic diagram of the adaptive oblique cylinder region restriction strategy in three-dimensional space for improving the Informed-RRT* algorithm;

[0027] Figure 7 Schematic diagram of the path segmentation restriction sampling strategy for improving the Informed-RRT* algorithm when the path is divided into different parts by sharp bends;

[0028] Figure 8 This is a schematic diagram of the working environment of the micro-assembly collaborative robot;

[0029] Fig. 9 The starting and target positions of the micro-assembly collaborative robot are shown in Figure 1.

[0030] Fig.10 Schematic diagram of three-dimensional path planning of RRT* algorithm;

[0031] Fig.11 Schematic diagram of the three-dimensional path planning for the improved Informed-RRT* algorithm. DETAILED DESCRIPTION

[0032] The present invention will be further described in detail below in conjunction with the accompanying drawings and specific embodiments.

[0033] The present invention provides a three-dimensional path planning method for a micro-assembly collaborative robot, such as Figure 1 As shown, the following steps are included:

[0034] S1. System initialization. System initialization includes the initialization of collaborative robot information, environmental information, the initialization of the collaborative robot's starting and ending points, and the initial search range. It also includes the initialization of each iteration step, the range judgment of the end point, the maximum number of iterations or iteration time, the initial sampling range, and the number of path segmentation segments n.

[0035] It should be noted that the environmental collision body information is Figure 8 As shown in the figure, the green grid envelops the collision body, which is used for collision detection. The starting and ending positions and the corresponding postures of the collaborative robot are shown in the figure. Fig. 9 As shown, Fig. 9 (a) is the starting point posture. Fig. 9(b) is the target point pose; the iteration step is set to 0.001 meters; the end point range is determined by: a sphere with the end point as the center and the iteration step as the radius. If the sampling point is in this area and the collaborative robot passes through the sampling point to the end point without collision, it is considered that a path is successfully planned; the maximum number of iterations is set to 50,000 times; the initial sampling range is as follows: Fig. 9 As shown in the black-bordered rectangle in the middle; according to the environment, the optimal path is roughly a sharp turn, and the path is divided into two sections, so the number of path segmentation segments n is set to 2+1=3.

[0036] S2, obtain the initial path through the RRT* algorithm;

[0037] S3, based on the improved Informed-RRT* algorithm, an adaptive oblique cylinder area restriction strategy in three-dimensional space and a path segment restriction sampling strategy for paths divided into different parts by sharp bends are introduced. After obtaining the initial solution, the oblique cylinder area sampling restriction is performed according to the planned optimal path, such as Figure 2 As shown, the specific steps include:

[0038] S3.1. Divide the current optimal path into n segments according to the set number of segment segments n, randomly select an integer m between 1 and n, and select two segment combinations of the path restriction according to the integer and the specified segment selection rules, specifically including the following steps:

[0039] S3.11. Determine the number of segmentation n of the path according to the general direction of the path, add 1 to the number of segmentation segments where the path is divided by sharp turns, and take 2+1=3, and divide the path into three segments;

[0040] S3.12. Select the path segment according to the m value and the specified segment selection rules, where the segment selection rules are shown in the following table:

[0041] Table 1 Rules for fragment selection

[0042]

[0043] Among them, m is the number of random segments, n is the number of set path segmentation segments, and x start is the starting point of the currently planned optimal path, x goal is the end point of the currently planned optimal path, c min is the path length of the currently planned optimal path, in the table [c min / n,2*c min / n] means that in the shortest path currently searched, the path length to the starting point is [c min / n,2*c min / n], x1 and x2 are points contained in the randomly selected path in the path segment, corresponding to the starting point and end point of the selected path segment.

[0044] S3.2, randomly select two points in the path within the two segment combinations, use the path segment between the two points as the reference of the restricted range of the oblique ellipsoid, the two points as the starting point and the end point of the segment, and the straight line passing through the two points as the median axis of the oblique cylinder;

[0045] S3.3. Calculate the absolute value of the coordinate system difference between the starting point and the end point of the path segment. The upper and lower bases of the oblique cylinder are perpendicular to the coordinate system corresponding to the maximum absolute value of the difference. The maximum and minimum values ​​of the path in this coordinate system are taken as the positions of the upper and lower bases of the oblique cylinder.

[0046] S3.4. Calculate the distances between the points in the fragment and the central axis in the two-dimensional planes parallel to the upper and lower bases of the oblique cylinder, and select the maximum value of the distance as the radius r of the oblique cylinder. max , construct the oblique cylinder sampling restriction area, such as Figure 3 As shown. max The solution formula is:

[0047] r max =argmax{p(σ(x))|σ(x0)=x1,σ(x num-1 )=x2}

[0048] Among them, σ represents the intercepted path segment, num represents the number of path points contained in the path segment, x represents the path point, and x1 and x2 are the starting point and end point of the corresponding selected path segment.

[0049] It should be noted that the restricted range of the oblique cylinder in three-dimensional space is as follows Figure 5 As shown; Figure 6 The necessity of improving the adaptive oblique cylinder region restriction strategy in the Informed-RRT* algorithm in three-dimensional space is explained. If the oblique cylinder is constructed in only one direction, it will lead to Figure 6 The situation (a) is as follows:

[0050]

[0051] Where |Δz| means that when the absolute value of the difference in the path z coordinate system is small or close to 0, the r of the oblique cylinder is max will approach infinity, and the limiting effect is poor. Figure 6 (b) is the improved oblique cylinder restriction area constructed after using the adaptive oblique cylinder area restriction strategy, and the restriction effect is better. Figure 7The necessity of improving the path segmentation restriction sampling strategy in the Informed-RRT* algorithm for the path divided into different parts by sharp bends is explained. When the path is divided into two segments and the path segmentation restriction sampling strategy is not used, such as Figure 7 As shown in (a), the volume of the constructed oblique cylinder is large, and the sampling area restriction effect is not good; Figure 7 (b) and Figure 7 (c) shows the path segmentation restriction sampling strategy when m=1 and m=2, and the restriction effect is better.

[0052] S3.5, random sampling and path optimization are performed within the sampling restriction area of ​​the oblique cylinder, such as Figure 4 As shown, the specific steps include:

[0053] S3.51. Randomly select a point within the range of the upper and lower bases of the coordinate system axis that is perpendicular to the upper and lower bases of the oblique cylinder. The coordinate value corresponding to the point on the axis is used as a coordinate value corresponding to the sampling point.

[0054] S3.52, from 0 to the radius r of the oblique cylinder max The random value r is taken in

[0055] S3.53, randomly select angle from 0 to 360 degrees;

[0056] S3.54. In the plane parallel to the upper and lower bases of the oblique cylinder and passing through the random point in S3.51, draw a circle with the intersection of the central axis of the oblique cylinder and the plane as the center and r as the radius. The point on the circle corresponding to the angle angle is the sampling point. The reference 0 degree corresponding to the angle angle can be a random position or a fixed position, both of which can meet the requirements of random sampling.

[0057] S4. When the optimal path is planned, or the set iteration time or number limit is reached, the path planning is stopped and the shortest path at that time is output as the planned path.

[0058] It should be noted that, as a comparison, Fig.10 This is a schematic diagram of the 3D path planning of the RRT* algorithm after 50,000 iterations. Fig.10 (a) is the top view, (b) is the oblique view, (c) is the front view, and (d) is the right view. The blue path in the figure is the random tree constructed by the RRT* algorithm, and the red path is the shortest path planned, with a path length of 0.8104984m. Fig.11 Schematic diagram of the 3D path planning for 50,000 iterations of the Informed-RRT* algorithm. After the path is planned, the sampling points are clustered around the planned path in the subsequent optimization process. Fig.11(a) is the top view, (b) is the oblique view, (c) is the front view, and (d) is the right view. The shortest path length shown in the figure is 0.6733008m.

[0059] S5. The path point position information of the planned path is transmitted to the collaborative robot. The collaborative robot obtains the angle of each joint through the inverse solution of the position information and completes the planning operation under the path.

[0060] The above embodiments are preferred implementation modes of the present invention, but the implementation modes of the present invention are not limited to the above embodiments. Any other changes, modifications, substitutions, combinations, and simplifications that do not deviate from the spirit and principles of the present invention should be equivalent replacement methods and are included in the protection scope of the present invention.

Claims

1. A three-dimensional path planning method for a micro-assembly collaborative robot, characterized in that: The following steps are involved: S1. System initialization. System initialization includes initializing the environment around the collaborative robot to determine the size and position of obstacles, and the starting and ending points of the collaborative robot's path; S2, based on the size and position of the obstacle, the starting point and end point of the collaborative robot path, the initial path is obtained through the RRT* algorithm; S3. Based on the application of Informed-RRT* algorithm in three-dimensional environment, an adaptive oblique cylinder area restriction strategy in three-dimensional space and a path segment restriction sampling strategy for the path divided into different parts by sharp bends are introduced to obtain sampling points restricted to a certain area on the initial path, and the path is optimized according to the sampling points; S4. When the optimal path is planned, or the set iteration time or number limit is reached, the path planning is stopped and the shortest path at that time is output as the planned path; S5. The path point position information of the planned path is transmitted to the collaborative robot. The collaborative robot obtains the angle of each joint through the inverse solution of the position information and completes the planning operation under the path.

2. A three-dimensional path planning method for a micro-assembly collaborative robot according to claim 1, characterized in that: The system initialization in S1 also includes initialization of each iteration step, end point range judgment, maximum number of iterations or iteration time, initial sampling range, and path segment number n.

3. A three-dimensional path planning method for a micro-assembly collaborative robot according to claim 1, characterized in that: The S3 comprises the following steps: S3.

1. For the adaptive oblique cylinder area restriction strategy in three-dimensional space and the path segment restriction sampling strategy for the path divided into different parts by sharp bends, the current optimal path is divided into n segments according to the set path segment number n, and an integer m is randomly selected between 1 and n, and two segment combinations of path restrictions are selected according to the integer m and the specified segment selection rules; S3.2, randomly select two points in the path within the two segment combinations, use the path segment between the two points as the reference of the restricted range of the oblique ellipsoid, the two points as the starting point and the end point of the segment, and the straight line passing through the two points as the median axis of the oblique cylinder; S3.

3. Calculate the absolute value of the coordinate system difference between the starting point and the end point of the path segment. The upper and lower bases of the oblique cylinder are perpendicular to the coordinate system corresponding to the maximum absolute value of the difference. The maximum and minimum values ​​of the path in this coordinate system are taken as the positions of the upper and lower bases of the oblique cylinder. S3.

4. Calculate the distances between the points in the fragment and the central axis in the two-dimensional planes parallel to the upper and lower bases of the oblique cylinder, and select the maximum value of the distance as the radius r of the oblique cylinder. max , construct the oblique cylinder sampling restriction area, where r max The solution formula is: r max =argmax{ρ(σ(x))|σ(x0)=x1,σ(x num-1 )=x2} Wherein, σ represents the intercepted path segment, num represents the number of path points contained in the path segment, x represents the path point, and x1 and x2 are the starting point and end point of the corresponding selected path segment; S3.

5. Perform random sampling and path optimization within the sampling restriction area of ​​the oblique cylinder.

4. A three-dimensional path planning method for a micro-assembly collaborative robot according to claim 1, characterized in that: The method corresponding to the path optimization according to the sampling points in S3 is the path optimization method of the RRT* algorithm, which includes the path optimization processes of ChooseParent and Rewire.

5. The three-dimensional path planning method of a micro-assembly collaborative robot according to claim 3, characterized in that: The S3.1 comprises the following steps: S3.11, determine the number of segmentation n of the path according to the general direction of the path, and add 1 to the number of segmentation segments of the path due to sharp turns; S3.

12. Select the path segment according to the m value and the specified segment selection rule, where the segment selection rule is as follows: Let x start is the starting point of the currently planned optimal path, x goal is the end point of the currently planned optimal path, c min is the path length of the currently planned optimal path, x1 and x2 are randomly selected path points in the path segment, corresponding to the starting point and end point of the selected path segment. When m = 1, x1 = x start , from [c min / n,2*c min / n] randomly selects a path point as x2; when m = 2, from [c min / n,2*c min / n] randomly selects a path point as x1 from the path segment of [2*c min / n,3*c min / n] randomly selects a path point as x2; and so on, until when m=n-1, select [(n-2)*c min / n,(n-1)*c min / n] randomly selects a path point as x1, x2 = x goal ; When m = n, let x1 = x start , x2=x goal , in order to ensure the probabilistic completeness of the algorithm.

6. A three-dimensional path planning method for a micro-assembly collaborative robot according to claim 3, characterized in that: The random sampling in the oblique cylindrical sampling restriction area of ​​S3.5 comprises the following steps: S3.

51. Randomly select a point within the range of the upper and lower bases of the coordinate system axis perpendicular to the upper and lower bases of the oblique cylinder, and the coordinate value of the point on the axis is used as a coordinate value corresponding to the sampling point; S3.52, from 0 to the radius r of the oblique cylinder max The random value r is taken in S3.53, randomly select angle from 0 to 360 degrees; S3.

54. In the plane parallel to the upper and lower bases of the oblique cylinder and passing through the random point in S3.51, draw a circle with the intersection of the median axis of the oblique cylinder and the plane as the center and r as the radius. The point on the circle corresponding to angle is the sampling point.

7. A three-dimensional path planning method for a micro-assembly collaborative robot according to claim 6, characterized in that: In the sampling points corresponding to angle angle in S3.54, the reference 0 degree corresponding to angle angle is a random position or a fixed position, which can meet the requirements of random sampling.

Images