Mechanical arm path planning method

Abstract

The invention discloses a mechanical arm path planning method, which belongs to the technical field of mechanical arm path planning, is used for planning a mechanical arm path, and comprises the following steps: acquiring parameter information and preset parameter information of a to-be-detected area, and positions of a starting point and a target point of a cooperative manipulator at the tail end of a mechanical arm; defining a state sampling space; performing oriented random sampling in the state sampling space according to the target deviation factor to obtain a random point; gravitational search is added in calculation of new nodes of the RRT algorithm; performing optimization processing on the found path by using a greedy algorithm, and performing smoothing processing on the found path by using a B spline curve; and initial moving target point location information of the mechanical arm tail end cooperative manipulator is obtained and analyzed, and the next moving target point location of the mechanical arm tail end cooperative manipulator is obtained. Compared with the prior art, the method has the advantages that a feasible path can be quickly found in a high-dimensional space, discretization is not needed, and a dynamic environment and uncertain factors can be processed.

Description

TECHNICAL FIELD

[0001] The application discloses a mechanical arm path planning method and belongs to the technical field of mechanical arm path planning. BACKGROUND

[0002] The origin of mechanical arm path planning technology can be traced back to the 1960s, and with the increasing demand for industrial automation, mechanical arms have gradually been applied to production lines. Early mechanical arms mainly complete tasks through simple point-to-point motion, and the path planning is relatively simple, mainly based on geometric and kinematic analysis. With the development of computer technology, the kinematic and dynamic models of mechanical arms have gradually improved, providing a more accurate mathematical basis for path planning. The D-H parameter method in the kinematic model is very complex to calculate and solve for complex mechanical arm structures; the constant curvature (CC) model may cause large errors in deformation and nonlinear motion under the constant curvature assumption; the variable curvature (VCC) model is complex and requires a large amount of computing resources in the solving process. In the dynamic model, the Newton-Euler method has a large number of coupling terms and is difficult to solve; the Lagrange method involves complex integral operations and has a large amount of calculation.

[0003] With the complication of mechanical arm application scenarios, traditional search algorithms face the problem of excessive calculation in high-dimensional space and complex environments. The application proposes a mechanical arm path planning method, which proposes a sampling-based RRT algorithm that is simple in principle and suitable for high-dimensional space. The algorithm gradually explores the space by randomly expanding the nodes of the tree until a collision-free path from the starting point to the ending point is found. It can quickly find a feasible path in high-dimensional space, especially suitable for complex environments and high-degree-of-freedom mechanical arm path planning; as the number of samples increases, the algorithm can find a feasible path with probability 1; without discretization, it is suitable for continuous space; it can handle dynamic environments and uncertain factors, and is suitable for real-time path planning. SUMMARY

[0004] The application aims to provide a mechanical arm path planning method to solve the problem of complex path planning model and large amount of calculation in the prior art.

[0005] A mechanical arm path planning method, comprising:

[0006] S1. Obtain the parameter information of the to-be-detected region and the preset parameter information, obtain the initial stretching target point of the mechanical arm according to the parameter information of the to-be-detected region, and after the mechanical arm reaches the initial stretching target point, obtain the target point information of the mechanical arm and analyze and process it to obtain the positions of the starting point X init and the target point X goat of the end collaborative manipulator of the mechanical arm;

[0007] S2. Define a state sampling space;

[0008] S3. Guiding random sampling in the state sampling space according to the target deviation factor to obtain a random point X rand ;

[0009] S4. For each newly generated point X near , the Euclidean distances of the point X near from the starting point X init and the target point X goat are calculated respectively, and a cost function is constructed, and the node with the minimum path cost is selected, and the gravitational search is added in the calculation of the new node X new of the RRT algorithm;

[0010] S5. Repeat S3 and S4 to construct a randomly generated tree, and when a child node in the randomly generated tree enters a specified target region, a path from the starting point X init to the target point X gobat is found in the randomly generated tree;

[0011] S6. The found path is optimized using a greedy algorithm, and the found path is smoothed using a B-spline curve;

[0012] S7. Obtain obstacle information, two-dimensional image information, three-dimensional image information, and structure point cloud data at the initial motion target point of the end effector of the collaborative manipulator, and analyze and process the obstacle information, two-dimensional image information, three-dimensional image information, and structure point cloud data to obtain the next motion target point of the end effector of the collaborative manipulator.

[0013] The target point information of the robot arm in S1 includes obstacle information, two-dimensional image information, three-dimensional image information, and structure point cloud data.

[0014] S3 includes: if the random point X rand obtained by sampling is located in the obstacle region, random sampling is performed again; if the random point X rand obtained by sampling is not in the obstacle region, the Euclidean distances between the random point X rand and all existing points on the random tree are calculated, and the nearest point is selected as the point X near .

[0015] S4 includes that the cost function is:

[0016] cost(X)=cost(X init ,X)+cost(X,X goal );

[0017] In the formula, cost(X init ,X) is the cost of the starting point X init to the current node X, and cost(X,X goal ) is the cost of the target point Xgoal The cost to the current node X, cost(X) is the starting point X init To the current node X and the target point X goal The total cost to reach the current node X.

[0018] S4 includes the new node X in the RRT algorithm new The gravitational search formula is added to the calculation of:

[0019]

[0020] Where s is the newly generated path from point X near To random point X rand A step size for directional growth, k1 and k2 are parameters used to adjust the search speed and search randomness, is the position vector of the random sampling point, is the position vector of the point closest to the generated random tree among all random sampling points, The position vector of the target point for path planning.

[0021] S4 includes judging the size relationship between k1 and k2:

[0022] When k1<k2, the random tree node approaches the target point and expands the corresponding step size toward the target point;

[0023] When k1≥k2, the random tree node moves to X near Direction expansion, generating X new .

[0024] Set the starting point X init and target point X goat Connect for crash testing.

[0025] If the starting point X init and target point X goat If a collision occurs during the collision test, it returns to the previous parent node, updates the parent node to the new target point, and re-sets the starting point X init and the new target point.

[0026] If the starting point X init and target point X goat If no collision occurs during the collision test, it means that the starting point X init To target point X goat The nodes between can be ignored, and the target point X goat Add it to the random tree, update it to the new starting point, reset the last node at the target point, and then reconnect it until it reaches the target point.

[0027] The found path is optimized using the third-order non-uniform B-spline interpolation method:

[0028] p(u) = d0(1-u) 3 + 3d1(1-u) 2 + 3d2u(1-u) + d3u 2 ;

[0029] In the formula, p(u) is a B-spline curve equation, d0, d1, d2 and d3 are four control points that determine the shape of the curve, wherein d0 and d3 are the starting point and the end point of the curve respectively, and u is the current curve parameter value when calculating the basis function value.

[0030] Compared with the prior art, the application has the following beneficial effects: by gradually exploring the space through random expansion of the nodes of the tree, a feasible path can be quickly found in a high-dimensional space, which is particularly suitable for path planning of a mechanical arm with complex environment and high degrees of freedom; as the number of samplings increases, the algorithm can find a feasible path with a probability of 1; without discretization, the algorithm is suitable for continuous space; dynamic environment and uncertain factors can be processed, and the algorithm is suitable for real-time path planning. BRIEF DESCRIPTION OF DRAWINGS

[0031] Figure 1 is a technical flowchart of the application. DETAILED DESCRIPTION

[0032] In order to make the purpose, technical scheme and advantages of the application clearer, the technical scheme in the application will be clearly and completely described below. Obviously, the described embodiments are part of the embodiments of the application, rather than all the embodiments. Based on the embodiments in the application, all other embodiments obtained by those skilled in the art without creative labor fall within the scope of protection of the application.

[0033] A mechanical arm path planning method, as shown in Figure 1 , comprises:

[0034] S1. Obtain parameter information of a to-be-detected region and preset parameter information, obtain an initial stretching target point of a mechanical arm according to the parameter information of the to-be-detected region, obtain target point information of the mechanical arm after the mechanical arm reaches the initial stretching target point, and analyze and process the target point information to obtain the positions of a starting point X init and a target point X goat of a collaborative manipulator at the end of the mechanical arm;

[0035] S2. Define a state sampling space;

[0036] S3. Perform directional random sampling in the state sampling space according to a target deviation factor to obtain a random point X rand ;

[0037] S4. For each newly generated point Xnear , respectively calculate the Euclidean distance of point X near and the starting point X init , the target point X goat and construct a cost function, select the node with the minimum path cost, add the gravitational search in the calculation of the new node X new of the RRT algorithm;

[0038] S5. Repeat S3, S4 to build a randomly generated tree, when the child node in the randomly generated tree enters the specified target area, find a path from the starting point X init to the target point X goat in the randomly generated tree;

[0039] S6. Use the greedy algorithm to optimize the found path, use the B-spline curve to smooth the found path;

[0040] S7. Obtain the obstacle information, two-dimensional image information, three-dimensional image information and structure point cloud data at the initial motion target point of the end of the collaborative manipulator of the robot arm and analyze and process them to obtain the next motion target point of the end of the collaborative manipulator of the robot arm.

[0041] The target point information of the robot arm in S1 includes obstacle information, two-dimensional image information, three-dimensional image information and structure point cloud data.

[0042] S3 includes: if the randomly sampled point X rand is located in the obstacle region, re-sampling is performed; if the randomly sampled point X rand is not in the obstacle region, calculate the Euclidean distance between the random point X rand and all existing points on the random tree, and select the nearest point as the point X near .

[0043] S4 includes that the cost function is:

[0044] cost(X)=cost(X init ,X)+cost(X,X goal );

[0045] In the formula, cost(X init ,X) is the cost from the starting point X init to the current node X, cost(X,X goal ) is the cost from the target point X goal to the current node X, and cost(X) is the total cost from the starting point X init to the current node X and from the target point X goal to the current node X.

[0046] S4 includes adding the gravitational search in the calculation of the new node Xnew The gravitational search formula is added to the calculation of:

[0047]

[0048] Where s is the newly generated path from point X near To random point X rand A step size for directional growth, k1 and k2 are parameters used to adjust the search speed and search randomness, is the position vector of the random sampling point, is the position vector of the point closest to the generated random tree among all random sampling points, The position vector of the target point for path planning.

[0049] S4 includes judging the size relationship between k1 and k2:

[0050] When k1<k2, the random tree node approaches the target point and expands the corresponding step size toward the target point;

[0051] When k1≥k2, the random tree node moves to X near Direction expansion, generating X new .

[0052] Set the starting point X init and target point X goat Connect for crash testing.

[0053] If the starting point X init and target point X goat If a collision occurs during the collision test, it returns to the previous parent node, updates the parent node to the new target point, and re-sets the starting point X init and the new target point.

[0054] If the starting point X init and target point X goat If no collision occurs during the collision test, it means that the starting point X init To target point X goat The nodes between can be ignored, and the target point X goat Add it to the random tree, update it to the new starting point, reset the last node at the target point, and then reconnect it until it reaches the target point.

[0055] The found path is optimized using the third-order non-uniform B-spline interpolation method:

[0056] p(u)=d0(1-u) 3 +3d1(1-u) 2 +3d2u(1-u)+d3u 2 ;

[0057] where p(u) is a B-spline curve equation, d0, d1, d2 and d3 are four control points that determine the shape of the curve, wherein d0 and d3 are the start point and the end point of the curve respectively, and u is a current curve parameter value when calculating the basis function value.

[0058] The above examples are only used to illustrate the technical solutions of the present application, but not to limit it. Although the present application has been described in detail with reference to the foregoing examples, it should be understood by those skilled in the art that the technical solutions recorded in the foregoing examples can be modified, or some or all of the technical features can be replaced equivalently, and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the scope of the technical solutions of the embodiments of the present application.

Claims

1. A robot arm path planning method, characterized in that: include: S1. Obtain parameter information and preset parameter information of the area to be detected, obtain the initial extension target point of the robot arm based on the parameter information of the area to be detected, obtain the target point information of the robot arm and analyze and process it after the robot arm reaches the initial extension target point, and obtain the starting point X of the collaborative manipulator at the end of the robot arm. init and target point X goat location; S2. Define the state sampling space; S3. Perform guided random sampling in the state sampling space according to the target deviation factor to obtain a random point X rand ; S4. For each newly generated point X near , calculate point X respectively near With the starting point X init 、Target point X goat The Euclidean distance and cost function are constructed to select the node with the minimum path cost. new Add gravitational search to the calculation; S5. Repeat S3 and S4 to construct a random spanning tree. When the child node in the random spanning tree enters the specified target area, find a path from the starting point X in the random spanning tree. init To target point X goat Path; S6. Optimize the found path using a greedy algorithm and smooth the found path using a B-spline curve; S7. Obtain obstacle information, two-dimensional image information, three-dimensional image information and structural point cloud data at the initial motion target point of the collaborative manipulator at the end of the robotic arm and analyze and process them to obtain the next motion target point of the collaborative manipulator at the end of the robotic arm.

2. A robot arm path planning method according to claim 1, characterized in that: The target point information of the robotic arm in S1 includes obstacle information, two-dimensional image information, three-dimensional image information and structure point cloud data.

3. A robot arm path planning method according to claim 2, characterized in that: S3 includes the random point X obtained by sampling rand If the random point X is within the obstacle area, random sampling is performed again; if the random point X rand Not in the obstacle area, calculate the random point X rand The Euclidean distance between all existing points on the random tree, select the point with the closest distance as point X near .

4. A robot arm path planning method according to claim 3, characterized in that: S4 includes the cost function: cost(X)=cost(X init ,X)+cost(X,X goal ); In the formula, cost(X init ,X) is the starting point X init The cost to the current node X, cost(X,X goal ) is the target point X goal The cost to the current node X, cost(X) is the starting point X init To the current node X and the target point X goal The total cost to reach the current node X.

5. A robot arm path planning method according to claim 4, characterized in that: S4 includes the new node X in the RRT algorithm new The gravitational search formula is added to the calculation of: Where s is the newly generated path from point X near To random point X rand A step size for directional growth, k1 and k2 are parameters used to adjust the search speed and search randomness, is the position vector of the random sampling point, is the position vector of the point closest to the generated random tree among all random sampling points, The position vector of the target point for path planning.

6. A robot arm path planning method according to claim 5, characterized in that: S4 includes judging the size relationship between k1 and k2: When k1<k2, the random tree node approaches the target point and expands the corresponding step size toward the target point; When k1≥k2, the random tree node moves to X near Direction expansion, generating X new .

7. A robot arm path planning method according to claim 6, characterized in that: Set the starting point X init and target point X goat Connect for crash testing.

8. A robot arm path planning method according to claim 7, characterized in that: If the starting point X init and target point X goat If a collision occurs during the collision test, it returns to the previous parent node, updates the parent node to the new target point, and re-sets the starting point X init and the new target point.

9. A robot arm path planning method according to claim 8, characterized in that: If the starting point X init and target point X goat If no collision occurs during the collision test, it means that the starting point X init To target point X goat The nodes between can be ignored, and the target point X goat Add it to the random tree, update it to the new starting point, reset the last node at the target point, and then reconnect it until it reaches the target point.

10. A robot arm path planning method according to claim 9, characterized in that: The found path is optimized using the third-order non-uniform B-spline interpolation method: p(u)=d0(1-u) 3 +3d1(1-u) 2 +3d2u(1-u)+d3u 2 ; Where p(u) is the B-spline curve equation, d0, d1, d2, and d3 are the four control points that determine the shape of the curve, where d0 and d3 are the starting and end points of the curve, respectively, and u is the current curve parameter value when calculating the basis function value.

Images