A robot arm path planning method

Abstract

The present invention provides a robot path planning method, which belongs to the field of robot control technology, and includes the following steps: obtaining a robot that needs to be path planned, and modeling the robot using an improved DH parameter method, constructing a forward kinematics model and an inverse kinematics model of the robot, and marking the singularity of the robot; obtaining the starting point and the end point of the end of the robot, and using the inverse kinematics model to solve the inverse kinematics solution of the starting point and the end point, performing collision detection on the solved inverse kinematics solution, and screening out a collision-free inverse kinematics solution; and using the IARRT‑Connect algorithm to perform path planning based on the collision-free inverse kinematics solution and the starting point joint angle and the end point joint angle corresponding to the robot. The present invention significantly reduces the search space, reduces the total cost of the path, and eliminates redundant paths in the random tree, further improving the efficiency of path planning.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot arm control, and in particular relates to a robot arm path planning method. Background Art

[0002] RRT algorithm is the abbreviation of Rapidly-exploring Random Tree algorithm, that is, rapidly expanding random tree algorithm.

[0003] At present, as a complex system with high precision, multi-input and multi-output, high nonlinearity and strong coupling, the robot arm has extremely high operational flexibility, which makes it widely used in industrial assembly, agricultural production, medical treatment, safety and explosion protection and other fields. The robot arm can accept control instructions during use and accurately locate a certain point in three-dimensional space to operate. The robot arm needs to plan the path during the operation. First, the optimal path can be selected through path planning, reducing unnecessary movement and waiting, thereby improving the operation efficiency. Secondly, path planning can also ensure that the robot arm avoids obstacles during movement and prevents collision with the surrounding environment. That is, path planning can protect the robot arm from damage and avoid damage to surrounding equipment and personnel. Thirdly, path planning can ensure the operation accuracy of the robot arm to achieve accurate arrival at the target operation position, especially to ensure the realization of high-precision operations.

[0004] Existing robot path planning methods usually use the RRT algorithm. RRT is a single-step query search algorithm based on random sampling. It does not need to model environmental obstacles, so it is very suitable for high-dimensional space path planning and has been widely used. However, the RRT algorithm has search blindness in robot path planning, and too many sampling points make it difficult to converge to the optimal solution, which brings certain troubles to robot path planning. With the development of path planning, there has been The execution of these algorithms can effectively improve the search blindness of the RRT algorithm, reduce the number of sampling points, increase the planning speed and optimize the path quality. However, although these improved algorithms can improve the performance of the RRT algorithm, they may also increase the complexity of the algorithm, resulting in a slower path planning speed.

[0005] This is a shortcoming of the prior art. Therefore, it is very necessary to provide a robot arm path planning method to address the above defects in the prior art. Summary of the invention

[0006] In view of the fact that robotic arms are currently widely used in various fields, path planning is needed during the operation of the robotic arms to improve operating efficiency, prevent collisions with the surrounding environment, and improve operating accuracy. The commonly used RRT algorithm has limitations, which restricts the development of robotic arm path planning. The improved RRT algorithm can improve the quality of path planning, but increases the complexity of the algorithm, resulting in the defect of too long path planning time. The present invention provides a robotic arm path planning method to solve the above technical problems.

[0007] The present invention provides a robot arm path planning method, comprising the following steps:

[0008] S1. Obtain the robot arm that needs path planning, and model the robot arm using the improved DH parameter method, build the forward kinematics model and inverse kinematics model of the robot arm, and mark the singular points of the robot arm;

[0009] S2. Obtain the starting point and the end point of the end of the robot arm, and use the inverse kinematics model to solve the inverse kinematics solution of the starting point and the end point, perform collision detection on the solved inverse kinematics solution, and select the collision-free inverse kinematics solution;

[0010] S3. Path planning is performed using the IARRT-Connect algorithm based on the collision-free inverse kinematics solution and the starting and ending joint angles corresponding to the robotic arm.

[0011] Furthermore, the specific steps of step S1 are as follows:

[0012] S11. Obtain the robot arm that needs to perform path planning, and determine the degree of freedom n and the length of n connecting rods of the robot arm;

[0013] S12. Using the improved DH parameter method, a coordinate system is established for each link at the front end of each link of the robot arm;

[0014] S13. Determine n transformation matrices of the robot arm according to the transformation matrix of the adjacent joint link coordinate system, and then multiply the n matrices in sequence to obtain the posture transformation matrix of the end effector of the robot arm and the base coordinate system; wherein the elements of the transformation matrix are DH parameters;

[0015] S14. Set the desired configuration of the robot arm, and calculate the angles of each joint as the inverse kinematics solution of the robot arm in combination with the forward kinematics formula of the robot arm;

[0016] S15. Calculate the singularity points of the robot arm based on the degrees of freedom of the robot arm and mark them.

[0017] Furthermore, the specific steps of step S2 are as follows:

[0018] S21. Solve the inverse kinematics solution in Cartesian coordinates according to the starting point and the end point of the end of the robot arm, and obtain N sets of inverse kinematics solutions for the starting point and the end point, where N is determined by the degree of freedom of the robot arm;

[0019] S22. performing collision detection on 2N sets of inverse kinematic solutions;

[0020] If all 2N sets of inverse kinematic solutions collide, it is determined that there is no reachable path and the process ends;

[0021] If there is a collision-free inverse kinematics solution, go to step S3.

[0022] Furthermore, the specific steps of step S22 are as follows:

[0023] S221. Use cylindrical bounding box to simulate obstacles, joints of the robot arm and the link envelope;

[0024] S222. Select one group from the 2N groups of inverse kinematic solutions, determine the positions of the axes of each bounding box of the robot corresponding to the selected current group of inverse kinematic solutions, and calculate the minimum distance d between the axes of each cylindrical bounding box min ;

[0025] S223. Get the radius of each cylindrical bounding box;

[0026] S224. Determine whether there is a minimum distance d between the axes of two cylindrical bounding boxes min Less than or equal to the sum of the radii of the two cylindrical bounding boxes;

[0027] If yes, it is determined that a collision will occur, and the process goes to step S226;

[0028] If not, it is determined that no collision will occur, and the process goes to step S225;

[0029] S225. Determine whether the angles of the joints of the robot arm corresponding to the inverse kinematics solution of the current group selected are singular points of the robot arm;

[0030] If yes, go to step S226;

[0031] If not, the current set of inverse kinematic solutions is stored as collision-free inverse kinematic solutions;

[0032] S226. discard the current set of inverse kinematic solutions;

[0033] S227. Determine whether the 2N groups of inverse kinematic solutions have been screened;

[0034] If yes, go to step S228;

[0035] If not, return to step S222;

[0036] S228. Determine whether there is a collision-free inverse kinematics solution among the 2N sets of inverse kinematics solutions;

[0037] If yes, go to step S3;

[0038] If not, it is determined that there is no reachable path and the process ends.

[0039] Furthermore, the specific steps of step S3 are as follows:

[0040] S31. Select one of the collision-free inverse kinematics solutions at the starting point of the robot arm, and use the joint angle corresponding to the selected collision-free inverse kinematics solution as the path starting point q start , and select one of the collision-free inverse kinematics solutions at the end point of the robot arm, and use the joint angle corresponding to the selected collision-free inverse kinematics solution as the path target point q goal ;

[0041] S32. At the path starting point q start and path target point q goal A random tree is established separately, and the two random trees are controlled to cross-expand according to the set expansion step length λ according to the number of nodes. During the expansion process, collision detection is performed and the latest expansion point is selected according to the size of the joint angle potential energy until the two random trees are expanded.

[0042] Furthermore, the specific steps of step S32 are as follows:

[0043] S321. Set the path starting point q start The random tree is used as the first random tree, and the path target point q goal The random tree of is used as the second random tree;

[0044] S322. Set the path starting point q start As the root node of the first random tree, the path target point q goal As the root node of the second random tree, and starting node expansion from the root node of the first random tree;

[0045] S323. Compare the number of nodes of the first random tree and the second random tree;

[0046] The number of nodes in the first random tree is less than or equal to the number of nodes in the second random tree, and the process goes to step S324;

[0047] The number of nodes in the first random tree is greater than the number of nodes in the second random tree, and the process goes to step S325;

[0048] S324. Expand nodes of the first random tree according to the set expansion step λ, perform collision detection on the expanded nodes, and select the expanded node that passes the collision detection and has the smallest joint angle potential energy as the latest expanded node of the first random tree, and proceed to step S326;

[0049] S325. Expand nodes of the second random tree according to the set expansion step λ, perform collision detection on the expanded nodes, and select the expanded node that passes the collision detection and has the smallest joint angle potential energy as the latest expanded node of the second random tree;

[0050] S326. Check whether the path planning is completed based on the relationship between the latest extension points of the two random trees and the extension step length λ.

[0051] Furthermore, the specific steps of step S324 are as follows:

[0052] S3241. Generate a random number rand and use q rand Characterize the sampling points and use the following sampling function for sampling:

[0053]

[0054] Wherein, m is a preset threshold;

[0055] When rand is greater than m, q rand =Randrom, i.e. randomly select sampling points q rand ;

[0056] When rand is less than or equal to m, set q goal is the sampling point q rand ;

[0057] S3242. Select the distance sampling point q on the first random tree rand The nearest point is set as the closest point q nearest1 ;

[0058] S3243. Find the closest point q according to the set expansion step λ nearest1 The three surrounding points of get the closest point q nearest1 The three surrounding joint angles of each corresponding joint angle are 3 n The surrounding joint angle combinations are set as set q1, where n is the number of joint angles of the robot arm;

[0059] S3244. For all joint angle combinations in set q1 and the closest point q nearest1 Connect the lines and perform collision detection on the lines to select non-collision joint angle combinations;

[0060] S3245. Calculate the total potential energy of each group of joint angles in the collision-free joint angle combination, compare the total potential energy of each group of joint angles, and select the joint angle combination with the smallest total potential energy as the latest expansion point q of the first random tree new1 ;

[0061] The specific steps of step S325 are as follows:

[0062] S3251. Get the latest extension point q of the first random tree new1 ;

[0063] S3252. Select the latest extension point q on the second random tree from the first random tree new1 The nearest point is set as the closest point q nearest2 ;

[0064] S3253. Find the closest point q according to the set expansion step λ nearest2 The three surrounding points of get the closest point q nearest2 The three surrounding joint angles of each corresponding joint angle, and 3 n The number of surrounding joint angle combinations q2, where n is the number of joint angles of the robot arm;

[0065] S3254. For all joint angle combinations in set q2 and the closest point q nearest2 Connect the lines and perform collision detection on the lines to select non-collision joint angle combinations;

[0066] S3255. Calculate the total potential energy of each group of joint angles in the collision-free joint angle combination, compare the total potential energy of each group of joint angles, and select the joint angle combination with the smallest total potential energy as the latest expansion point q of the second random tree new2 .

[0067] Furthermore, the specific steps of step S3244 are as follows:

[0068] S32441. Combine all joint angles in set q1 with the closest point q nearest1 Connect the lines and divide them into w parts. Calculate the joint angle nodes according to the following formula :

[0069] ;

[0070] S32442. Use cylindrical bounding boxes to simulate obstacles, robot arm joints and link envelopes;

[0071] S32443. Using joint angle nodes A set of joint angles in the robot determines the position of the axes of each bounding box, and calculates the minimum distance d between the axes of each principle bounding box. min ;

[0072] S32444. Get the radius of each cylindrical bounding box;

[0073] S32445. Determine whether there is a minimum distance d between the axes of two cylindrical bounding boxes min Less than or equal to the sum of the radii of the two cylindrical bounding boxes;

[0074] If it exists, it is determined that a collision will occur and the current group of joint angles is discarded;

[0075] If it does not exist, it is determined that no collision will occur, and the current group of joint angles is stored as a non-collision joint angle combination. ;

[0076] The specific steps of step S3254 are as follows:

[0077] S32541. Combine all joint angles in set q2 with the closest point q nearest1 Connect the lines and divide them into w parts. Calculate the joint angle nodes according to the following formula :

[0078] ;

[0079] S32542. Use cylindrical bounding boxes to simulate obstacles, robot arm joints and link envelopes;

[0080] S32543. Use joint angle nodes A set of joint angles in the robot determines the position of the axes of each bounding box, and calculates the minimum distance d between the axes of each principle bounding box. min ;

[0081] S32544. Get the radius of each cylindrical bounding box;

[0082] S32545. Determine whether there is a minimum distance d between the axes of two cylindrical bounding boxes min Less than or equal to the sum of the radii of the two cylindrical bounding boxes;

[0083] If it exists, it is determined that a collision will occur and the current group of joint angles is discarded;

[0084] If it does not exist, it is determined that no collision will occur, and the current group of joint angles is stored as a non-collision joint angle combination. .

[0085] Furthermore, the specific steps of step S3245 are as follows:

[0086] S32451. Select the non-collision joint angle combination The gravitational potential energy is calculated using the following formula:

[0087]

[0088] in, is the gravitational proportionality coefficient;

[0089] S32452. Select the non-collision joint angle combination The repulsive potential energy is calculated by the following formula:

[0090]

[0091] in, is the repulsive force proportional coefficient, is the minimum distance between the robot arm and the obstacle, is the distance threshold affected by the repulsive potential field;

[0092] S32453. Add the gravitational potential energy and the repulsive potential energy to get the total potential energy U1:

[0093] ;

[0094] The specific steps of step S3255 are as follows:

[0095] S32551. Select the non-collision joint angle combination The gravitational potential energy is calculated using the following formula:

[0096]

[0097] in, is the gravitational proportionality coefficient;

[0098] S32552. Select the non-collision joint angle combination The repulsive potential energy is calculated by the following formula:

[0099]

[0100] in, is the repulsive force proportional coefficient, is the minimum distance between the robot arm and the obstacle, is the distance threshold affected by the repulsive potential field;

[0101] S32553. Add the gravitational potential energy and the repulsive potential energy to get the total potential energy U2:

[0102] .

[0103] Furthermore, the specific steps of step S326 are as follows:

[0104] S3261. Calculate the latest extension point q of the first random tree new1 The latest extension point q of the second random tree new2 Distance Relationship with expansion step size λ;

[0105] like , proceed to step S3263;

[0106] like , proceed to step S3262;

[0107] S3262. Determine that the path planning is not completed, and return to step S323;

[0108] S3263. Determine that path planning is completed.

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

[0110] The robot path planning method provided by the present invention performs robot path planning through the IARRT-Connect algorithm optimized by the RRT algorithm, and significantly reduces the search space by randomly expanding from the starting point and the target point in both directions, eliminates redundant paths in the random tree, and further improves the efficiency of the path.

[0111] In addition, the invention has a reliable design principle, a simple structure and a very broad application prospect.

[0112] It can be seen that compared with the prior art, the present invention has outstanding substantive features and significant progress, and the beneficial effects of its implementation are also obvious. BRIEF DESCRIPTION OF THE DRAWINGS

[0113] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, for ordinary technicians in this field, other drawings can be obtained based on these drawings without paying any creative work.

[0114] Figure 1 It is a flow chart of the robot arm path planning method of the present invention.

[0115] Figure 2 It is a schematic diagram of the connecting rod coordinate system of the six-degree-of-freedom mechanical arm of the present invention.

[0116] Figure 3 It is a schematic diagram of collision detection in the robot arm path planning method of the present invention. DETAILED DESCRIPTION

[0117] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of the present invention.

[0118] See also Figure 1 The figure shows a specific implementation of a robot arm path planning method, comprising the following steps:

[0119] S1. Obtain the robot arm that needs path planning, and model the robot arm using the improved DH parameter method, build the forward kinematics model and inverse kinematics model of the robot arm, and mark the singular points of the robot arm;

[0120] S2. Obtain the starting point and the end point of the end of the robot arm, and use the inverse kinematics model to solve the inverse kinematics solution of the starting point and the end point, perform collision detection on the solved inverse kinematics solution, and select the collision-free inverse kinematics solution;

[0121] S3. Path planning is performed using the IARRT-Connect algorithm based on the collision-free inverse kinematics solution and the starting and ending joint angles corresponding to the robotic arm.

[0122] In this embodiment, the specific steps of step S1 are as follows:

[0123] S11. Obtain the robot arm that needs to perform path planning, and determine the degree of freedom n and the length of n connecting rods of the robot arm;

[0124] S12. Use the improved DH parameter method to establish a coordinate system for each link at the front end of each link of the robot arm; Figure 2 Taking the six-degree-of-freedom robot shown as an example, n=6;

[0125] S13. Determine n transformation matrices of the robot arm according to the transformation matrix of the adjacent joint link coordinate system, and then multiply the n matrices in sequence to obtain the posture transformation matrix of the end effector of the robot arm and the base coordinate system; wherein the elements of the transformation matrix are DH parameters;

[0126] S14. Set the desired configuration of the robot arm, and calculate the angles of each joint as the inverse kinematics solution of the robot arm in combination with the forward kinematics formula of the robot arm;

[0127] S15. Calculate the singular points of the robot arm based on the degrees of freedom of the robot arm and mark them;

[0128] The specific steps of step S2 are as follows:

[0129] S21. Solve the inverse kinematics solution in Cartesian coordinates according to the starting point and the end point of the robot arm, and obtain N sets of inverse kinematics solutions for the starting point and the end point, where N is determined by the degree of freedom of the robot arm; taking a six-degree-of-freedom robot arm as an example, eight sets of inverse kinematics solutions for the starting point and the end point are obtained;

[0130] S22. performing collision detection on 2N sets of inverse kinematic solutions;

[0131] If all 2N sets of inverse kinematic solutions collide, it is determined that there is no reachable path and the process ends;

[0132] If there is a collision-free inverse kinematics solution, proceed to step S3;

[0133] The specific steps of step S22 are as follows:

[0134] S221. Use cylindrical bounding boxes to simulate obstacles, robot arm joints and link envelopes, such as Figure 3 As shown;

[0135] S222. Select one group from the 2N groups of inverse kinematic solutions, determine the positions of the axes of each bounding box of the robot corresponding to the selected current group of inverse kinematic solutions, and calculate the minimum distance d between the axes of each cylindrical bounding box min ;

[0136] S223. Get the radius of each cylindrical bounding box;

[0137] S224. Determine whether there is a minimum distance d between the axes of two cylindrical bounding boxes min Less than or equal to the sum of the radii of the two cylindrical bounding boxes;

[0138] If yes, it is determined that a collision will occur, and the process goes to step S226;

[0139] If not, it is determined that no collision will occur, and the process goes to step S225;

[0140] For example, for obstacle 1, the vertical distance from the center P of the circle in the same plane as the axis of connecting rod i to the axis of the cylindrical bounding box of connecting rod i is d min , and the radius r of obstacle 1 is taken as r1, and the radius of the bottom circle of the cylindrical bounding box of connecting rod i is taken as r2;

[0141] If d min >r1+r2, it is determined that obstacle 1 does not collide with connecting rod i;

[0142] If d min ≤r1+r2, it is determined that obstacle 1 will collide with connecting rod i;

[0143] S225. Determine whether the angles of the joints of the robot arm corresponding to the inverse kinematics solution of the current group selected are singular points of the robot arm;

[0144] If yes, go to step S226;

[0145] If not, the current set of inverse kinematic solutions is stored as collision-free inverse kinematic solutions;

[0146] S226. discard the current set of inverse kinematic solutions;

[0147] S227. Determine whether the 2N groups of inverse kinematic solutions have been screened;

[0148] If yes, go to step S228;

[0149] If not, return to step S222;

[0150] S228. Determine whether there is a collision-free inverse kinematics solution among the 2N sets of inverse kinematics solutions;

[0151] If yes, proceed to step S3; the specific steps of step S3 are as follows:

[0152] S31. Select one of the collision-free inverse kinematics solutions at the starting point of the robot arm, and use the joint angle corresponding to the selected collision-free inverse kinematics solution as the path starting point q start , and select one of the collision-free inverse kinematics solutions at the end point of the robot arm, and use the joint angle corresponding to the selected collision-free inverse kinematics solution as the path target point q goal ;

[0153] It should be noted that the first three joint angles of a collision-free inverse kinematics solution are usually selected as the path starting point q start Or as the path target point q goal ;

[0154] S32. At the path starting point q start and path target point q goal Establish a random tree respectively, control the two random trees to cross-expand according to the set expansion step length λ according to the number of nodes, perform collision detection during the expansion process, and select the latest expansion point according to the joint angle potential energy until the two random trees are expanded; the specific steps of step S32 are as follows:

[0155] S321. Set the path starting point q start The random tree is used as the first random tree, and the path target point q goal The random tree of is used as the second random tree;

[0156] S322. Set the path starting point q start As the root node of the first random tree, the path target point q goal As the root node of the second random tree, and starting node expansion from the root node of the first random tree;

[0157] S323. Compare the number of nodes of the first random tree and the second random tree;

[0158] The number of nodes in the first random tree is less than or equal to the number of nodes in the second random tree, and the process goes to step S324;

[0159] The number of nodes in the first random tree is greater than the number of nodes in the second random tree, and the process goes to step S325;

[0160] S324. Expand nodes of the first random tree according to the set expansion step λ, perform collision detection on the expanded nodes, and select the expanded node that passes the collision detection and has the largest joint angle potential energy as the latest expanded node of the first random tree, and proceed to step S326;

[0161] The specific steps of step S324 are as follows:

[0162] S3241. Generate a random number rand and use q rand Characterize the sampling points and use the following sampling function for sampling:

[0163]

[0164] Wherein, m is a preset threshold;

[0165] When rand is greater than m, q rand =Randrom, i.e. randomly select sampling points q rand ;

[0166] When rand is less than or equal to m, set q goal is the sampling point q rand ;

[0167] S3242. Select the distance sampling point q on the first random tree rand The nearest point is set as the closest point q nearest1 ;

[0168] S3243. Find the closest point q according to the set expansion step λ nearest1 The three surrounding points of get the closest point q nearest1 The three surrounding joint angles of each corresponding joint angle are 3 n The surrounding joint angle combinations are set as set q1, where n is the number of joint angles of the robot arm;

[0169] The first three joint angles of a collision-free inverse kinematics solution are selected as the path starting point q start Or as the path target point q goal， The closest point corresponds to 3 3 Peripheral joint angle combination;

[0170] S3244. For all joint angle combinations in set q1 and the closest point q nearest1 Connect the lines and perform collision detection on the lines to select non-collision joint angle combinations;

[0171] The specific steps of step S3244 are as follows:

[0172] S32441. Combine all joint angles in set q1 with the closest point q nearest1Connect the lines and divide them into w parts. Calculate the joint angle nodes according to the following formula :

[0173] ;

[0174] S32442. Use cylindrical bounding boxes to simulate obstacles, robot arm joints and link envelopes;

[0175] S32443. Using joint angle nodes A set of joint angles in the robot determines the position of the axes of each bounding box, and calculates the minimum distance d between the axes of each principle bounding box. min ;

[0176] S32444. Get the radius of each cylindrical bounding box;

[0177] S32445. Determine whether there is a minimum distance d between the axes of two cylindrical bounding boxes min Less than or equal to the sum of the radii of the two cylindrical bounding boxes;

[0178] If it exists, it is determined that a collision will occur and the current group of joint angles is discarded;

[0179] If it does not exist, it is determined that no collision will occur, and the current group of joint angles is stored as a non-collision joint angle combination. ;

[0180] S3245. Calculate the total potential energy of each group of joint angles in the collision-free joint angle combination, compare the total potential energy of each group of joint angles, and select the joint angle combination with the smallest total potential energy as the latest expansion point q of the first random tree new1 ;

[0181] The specific steps of step S3245 are as follows:

[0182] S32451. Select the non-collision joint angle combination The gravitational potential energy is calculated using the following formula:

[0183]

[0184] in, is the gravitational proportionality coefficient;

[0185] S32452. Select the non-collision joint angle combination The repulsive potential energy is calculated by the following formula:

[0186]

[0187] in, is the repulsive force proportional coefficient, is the minimum distance between the robot arm and the obstacle, is the distance threshold affected by the repulsive potential field;

[0188] S32453. Add the gravitational potential energy and the repulsive potential energy to get the total potential energy U1:

[0189] ;

[0190] S325. Expand nodes of the second random tree according to the set expansion step λ, perform collision detection on the expanded nodes, and select the expanded nodes that pass the collision detection and have the largest and smallest joint angle potential energy as the latest expanded nodes of the second random tree;

[0191] The specific steps of step S325 are as follows:

[0192] S3251. Get the latest extension point q of the first random tree new1 ;

[0193] S3252. Select the latest extension point q on the second random tree from the first random tree new1 The nearest point is set as the closest point q nearest2 ;

[0194] S3253. Find the closest point q according to the set expansion step λ nearest2 The three surrounding points of the closest point q nearest2 The three surrounding joint angles of each corresponding joint angle, and 3 n The number of surrounding joint angle combinations q2, where n is the number of joint angles of the robot arm;

[0195] S3254. For all joint angle combinations in set q2 and the closest point q nearest2 Connect the lines and perform collision detection on the lines to select non-collision joint angle combinations;

[0196] The specific steps of step S3254 are as follows:

[0197] S32541. Combine all joint angles in set q2 with the closest point q nearest1 Connect the lines and divide them into w parts. Calculate the joint angle nodes according to the following formula :

[0198] ;

[0199] For example, w can be 100;

[0200] S32542. Use cylindrical bounding boxes to simulate obstacles, robot arm joints and link envelopes;

[0201] S32543. Use joint angle nodes A set of joint angles in the robot determines the position of the axes of each bounding box, and calculates the minimum distance d between the axes of each principle bounding box. min ;

[0202] S32544. Get the radius of each cylindrical bounding box;

[0203] S32545. Determine whether there is a minimum distance d between the axes of two cylindrical bounding boxes min Less than or equal to the sum of the radii of the two cylindrical bounding boxes;

[0204] If it exists, it is determined that a collision will occur and the current group of joint angles is discarded;

[0205] If it does not exist, it is determined that no collision will occur, and the current group of joint angles is stored as a non-collision joint angle combination. ;

[0206] S3255. Calculate the total potential energy of each group of joint angles in the collision-free joint angle combination, compare the total potential energy of each group of joint angles, and select the joint angle combination with the smallest total potential energy as the latest expansion point q of the second random tree new2 ;

[0207] The specific steps of step S3255 are as follows:

[0208] S32551. Select the non-collision joint angle combination The gravitational potential energy is calculated using the following formula:

[0209]

[0210] in, is the gravitational proportionality coefficient;

[0211] S32552. Select the non-collision joint angle combination The repulsive potential energy is calculated by the following formula:

[0212]

[0213] in, is the repulsive force proportional coefficient, is the minimum distance between the robot arm and the obstacle, is the distance threshold affected by the repulsive potential field;

[0214] S32553. Add the gravitational potential energy and the repulsive potential energy to get the total potential energy U2:

[0215] ;

[0216] S326. Detect whether the path planning is completed based on the relationship between the latest extension points of the two random trees and the extension step length λ; the specific steps of step S326 are as follows:

[0217] S3261. Calculate the latest extension point q of the first random tree new1 The latest extension point q of the second random tree new2 Distance Relationship with the expansion step size λ;

[0218] like , proceed to step S3263;

[0219] like , proceed to step S3262;

[0220] S3262. Determine that the path planning is not completed, and return to step S323;

[0221] S3263. Determine that path planning is completed.

[0222] It should be understood that the order of execution of the steps in the above embodiment does not mean that the execution order of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiment of the present invention.

[0223] Although the present invention has been described in detail with reference to the accompanying drawings and in combination with the preferred embodiments, the present invention is not limited thereto. Without departing from the spirit and essence of the present invention, a person of ordinary skill in the art may make various equivalent modifications or substitutions to the embodiments of the present invention, and these modifications or substitutions shall be within the scope of the present invention. Any person of ordinary skill in the art may easily think of changes or substitutions within the technical scope disclosed by the present invention, and these shall be within the scope of protection of the present invention.

Claims

1. A robot arm path planning method, characterized in that: The steps include: S1. Obtain the robot arm that needs path planning, and model the robot arm using the improved DH parameter method, build the forward kinematics model and inverse kinematics model of the robot arm, and mark the singular points of the robot arm; S2. Obtain the starting point and the end point of the end of the robot arm, and use the inverse kinematics model to solve the inverse kinematics solution of the starting point and the end point, perform collision detection on the solved inverse kinematics solution, and select the collision-free inverse kinematics solution; S3. Path planning is performed using the IARRT-Connect algorithm based on the collision-free inverse kinematics solution and the starting joint angle and the ending joint angle corresponding to the robot arm; the specific steps of step S3 are as follows: S31. Select one of the collision-free inverse kinematics solutions at the starting point of the robot arm, and use the joint angle corresponding to the selected collision-free inverse kinematics solution as the path starting point q start , and select one of the collision-free inverse kinematics solutions at the end point of the robot arm, and use the joint angle corresponding to the selected collision-free inverse kinematics solution as the path target point q goal ; S32. At the path starting point q start and the path target point q goal Establish a random tree respectively, control the two random trees to cross-expand according to the set expansion step length λ according to the number of nodes, perform collision detection during the expansion process, and select the latest expansion point according to the joint angle potential energy until the two random trees are expanded; the specific steps of step S32 are as follows: S321. Set the path starting point q start The random tree is used as the first random tree, and the path target point q goal The random tree of is used as the second random tree; S322. Set the path starting point q start As the root node of the first random tree, the path target point q goal As the root node of the second random tree, and starting node expansion from the root node of the first random tree; S323. Compare the number of nodes of the first random tree and the second random tree; The number of nodes in the first random tree is less than or equal to the number of nodes in the second random tree, and the process goes to step S324; The number of nodes in the first random tree is greater than the number of nodes in the second random tree, and the process goes to step S325; S324. Expand nodes of the first random tree according to the set expansion step λ, perform collision detection on the expanded nodes, and select the expanded node that passes the collision detection and has the smallest joint angle potential energy as the latest expanded node of the first random tree, and proceed to step S326; the specific steps of step S324 are as follows: S3241. Generate a random number rand and use q rand Characterize the sampling points and use the following sampling function for sampling: Wherein, m is a preset threshold; When rand is greater than m, q rand =Randrom, i.e. randomly select sampling points q rand ; When rand is less than or equal to m, set q goal is the sampling point q rand ; S3242. Select the distance sampling point q on the first random tree rand The nearest point is set as the closest point q nearest1 ; S3243. Find the closest point q according to the set expansion step λ nearest1 The three surrounding points of the closest point q nearest1 The three surrounding joint angles of each corresponding joint angle are 3 n The surrounding joint angle combinations are set as set q1, where n is the number of joint angles of the robot arm; S3244. For all joint angle combinations in set q1 and the closest point q nearest1 Connect the lines and perform collision detection on the lines to select non-collision joint angle combinations; S3245. Calculate the total potential energy of each group of joint angles in the collision-free joint angle combination, compare the total potential energy of each group of joint angles, and select the joint angle combination with the smallest total potential energy as the latest expansion point q of the first random tree new1 ; S325. Expand nodes of the second random tree according to the set expansion step λ, perform collision detection on the expanded nodes, and select the expanded node that passes the collision detection and has the smallest joint angle potential energy as the latest expanded node of the second random tree; the specific steps of step S325 are as follows: S3251. Get the latest extension point q of the first random tree new1 ; S3252. Select the latest extension point q on the second random tree from the first random tree new1 The nearest point is set as the closest point q nearest2 ; S3253. Find the closest point q according to the set expansion step λ nearest2 The three surrounding points of the closest point q nearest2 The three surrounding joint angles of each corresponding joint angle, and 3 n The number of surrounding joint angle combinations q2, where n is the number of joint angles of the robot arm; S3254. For all joint angle combinations in set q2 and the closest point q nearest2 Connect the lines and perform collision detection on the lines to select non-collision joint angle combinations; S3255. Calculate the total potential energy of each group of joint angles in the collision-free joint angle combination, compare the total potential energy of each group of joint angles, and select the joint angle combination with the smallest total potential energy as the latest expansion point q of the second random tree new2 ; S326. Check whether the path planning is completed based on the relationship between the latest extension points of the two random trees and the extension step length λ.

2. The robot arm path planning method according to claim 1, characterized in that: The specific steps of step S1 are as follows: S11. Obtain the robot arm that needs to perform path planning, and determine the degree of freedom n and the length of n connecting rods of the robot arm; S12. Using the improved DH parameter method, a coordinate system is established for each link at the front end of each link of the robot arm; S13. Determine n transformation matrices of the robot arm according to the transformation matrix of the adjacent joint link coordinate system, and then multiply the n matrices in sequence to obtain the posture transformation matrix of the end effector of the robot arm and the base coordinate system; wherein the elements of the transformation matrix are DH parameters; S14. Set the desired configuration of the robot arm, and calculate the angles of each joint as the inverse kinematics solution of the robot arm in combination with the forward kinematics formula of the robot arm; S15. Calculate the singularity points of the robot arm based on the degrees of freedom of the robot arm and mark them.

3. The robot arm path planning method according to claim 2, characterized in that: The specific steps of step S2 are as follows: S21. Solve the inverse kinematics solution in Cartesian coordinates according to the starting point and the end point of the end of the robot arm, and obtain N sets of inverse kinematics solutions for the starting point and the end point, where N is determined by the degree of freedom of the robot arm; S22. performing collision detection on 2N sets of inverse kinematic solutions; If all 2N sets of inverse kinematic solutions collide, it is determined that there is no reachable path and the process ends; If there is a collision-free inverse kinematics solution, go to step S3.

4. The robot arm path planning method according to claim 3, characterized in that: The specific steps of step S22 are as follows: S221. Use cylindrical bounding box to simulate obstacles, joints of the robot arm and the link envelope; S222. Select one group from the 2N groups of inverse kinematic solutions, determine the positions of the axes of each bounding box of the robot corresponding to the selected current group of inverse kinematic solutions, and calculate the minimum distance d between the axes of each cylindrical bounding box min ; S223. Get the radius of each cylindrical bounding box; S224. Determine whether there is a minimum distance d between the axes of two cylindrical bounding boxes min Less than or equal to the sum of the radii of the two cylindrical bounding boxes; If yes, it is determined that a collision will occur, and the process goes to step S226; If not, it is determined that no collision will occur, and the process goes to step S225; S225. Determine whether the angles of the joints of the robot arm corresponding to the inverse kinematics solution of the current group selected are singular points of the robot arm; If yes, go to step S226; If not, the current set of inverse kinematic solutions is stored as collision-free inverse kinematic solutions; S226. discard the current set of inverse kinematic solutions; S227. Determine whether the 2N groups of inverse kinematic solutions have been screened; If yes, go to step S228; If not, return to step S222; S228. Determine whether there is a collision-free inverse kinematics solution among the 2N sets of inverse kinematics solutions; If yes, go to step S3; If not, it is determined that there is no reachable path and the process ends.

5. The robot arm path planning method according to claim 3, characterized in that: The specific steps of step S3244 are as follows: S32441. Combine all joint angles in set q1 with the closest point q nearest1 Connect the lines and divide them into w parts. Calculate the joint angle nodes according to the following formula : ； S32442. Use cylindrical bounding boxes to simulate obstacles, robot arm joints and link envelopes; S32443. Using joint angle nodes A set of joint angles in the robot determines the position of the axes of each bounding box, and calculates the minimum distance d between the axes of each principle bounding box. min ; S32444. Get the radius of each cylindrical bounding box; S32445. Determine whether there is a minimum distance d between the axes of two cylindrical bounding boxes min Less than or equal to the sum of the radii of the two cylindrical bounding boxes; If it exists, it is determined that a collision will occur and the current group of joint angles is discarded; If it does not exist, it is determined that no collision will occur, and the current group of joint angles is stored as a non-collision joint angle combination. ; The specific steps of step S3254 are as follows: S32541. Combine all joint angles in set q2 with the closest point q nearest1 Connect the lines and divide them into w parts. Calculate the joint angle nodes according to the following formula : ； S32542. Use cylindrical bounding boxes to simulate obstacles, robot arm joints and link envelopes; S32543. Use joint angle nodes A set of joint angles in the robot determines the position of the axes of each bounding box, and calculates the minimum distance d between the axes of each principle bounding box. min ; S32544. Get the radius of each cylindrical bounding box; S32545. Determine whether there is a minimum distance d between the axes of two cylindrical bounding boxes min Less than or equal to the sum of the radii of the two cylindrical bounding boxes; If it exists, it is determined that a collision will occur and the current group of joint angles is discarded; If it does not exist, it is determined that no collision will occur, and the current group of joint angles is stored as a non-collision joint angle combination. .

6. The robot arm path planning method according to claim 5, characterized in that: The specific steps of step S3245 are as follows: S32451. Select the non-collision joint angle combination The gravitational potential energy is calculated using the following formula: in, is the gravitational proportionality coefficient; S32452. Select the non-collision joint angle combination The repulsive potential energy is calculated by the following formula: in, is the repulsive force proportional coefficient, is the minimum distance between the robot arm and the obstacle, is the distance threshold affected by the repulsive potential field; S32453. Add the gravitational potential energy and the repulsive potential energy to get the total potential energy U1: ； The specific steps of step S3255 are as follows: S32551. Select the non-collision joint angle combination The gravitational potential energy is calculated using the following formula: in, is the gravitational proportionality coefficient; S32552. Select the non-collision joint angle combination The repulsive potential energy is calculated by the following formula: in, is the repulsive force proportional coefficient, is the minimum distance between the robot arm and the obstacle, is the distance threshold affected by the repulsive potential field; S32553. Add the gravitational potential energy and the repulsive potential energy to get the total potential energy U2: 。 7. The robot arm path planning method according to claim 5, characterized in that: The specific steps of step S326 are as follows: S3261. Calculate the latest extension point q of the first random tree new1 The latest extension point q of the second random tree new2 Distance Relationship with the expansion step size λ; like , proceed to step S3263; like , proceed to step S3262; S3262. Determine that the path planning is not completed, and return to step S323; S3263. Determine that path planning is completed.

Images