A mechanical arm motion planning method based on configuration space sampling algorithm

Abstract

The application discloses a mechanical arm motion planning method based on a configuration space sampling algorithm, and the method comprises the following steps: determining a mechanical arm kinematic model; initializing a configuration space, determining joint angles corresponding to a starting pose and a target pose; mapping a map in a Cartesian space to the configuration space; and searching for a collision-free path in the configuration space according to an optimized sampling algorithm. The application adopts the strategy of constructing a gravitational potential field, performing secondary sampling and expanding a target node to guide node expansion, and the strategy of adaptive step length, dynamic target bias and dynamic retrieval is used to optimize the expansion process, so that the planning can be completed in a short time, the path cost is reduced, and the planning efficiency and planning effect of the algorithm are significantly improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robots, in particular to a mechanical arm motion planning method based on a configuration space sampling algorithm. BACKGROUND

[0002] In the mechanical arm workspace, there are often some obstacles affecting the normal motion of the mechanical arm. The traditional teaching method is low in efficiency for path planning of the mechanical arm in a complex scene, so it is necessary to introduce an intelligent path planning algorithm to improve the path planning efficiency of the mechanical arm.

[0003] Although the common rapid expansion random tree algorithm is widely used in mechanical arm path planning, the algorithm has the problems of strong randomness and invalid search, resulting in very low planning efficiency. SUMMARY

[0004] The technical problem to be solved by the present application is to overcome the shortcomings of the prior art, and to provide a mechanical arm motion planning method based on a configuration space sampling algorithm, which can solve the technical problems of low planning efficiency and large path cost of the traditional rapid expansion random tree algorithm.

[0005] To achieve the above purpose, the present application adopts the following technical scheme:

[0006] In a first aspect, the present application provides a mechanical arm motion planning method based on a configuration space sampling algorithm, comprising the following steps:

[0007] Step 1: kinematic modeling according to the mechanical arm configuration and geometric parameters, establishing a kinematic model;

[0008] Step 2: initializing the configuration space, based on the established kinematic model, solving the joint angles corresponding to the starting state and the target state of the mechanical arm as the starting node and the target node of the mechanical arm in the configuration space;

[0009] Step 3: mapping the collision relationship between the mechanical arm and the obstacle in the Cartesian space to the configuration space by the Monte Carlo method;

[0010] Step 4: based on the configuration space and the starting node and the target node of the mechanical arm in the configuration space, expanding the nodes by a sampling strategy, and reselecting the parent nodes and reconnecting the regions for the expanded new nodes, and cyclically executing the node expansion until the expanded new nodes enter the inflation range of the target node, generating a collision-free path.

[0011] Further, in step 1, the kinematic modeling according to the mechanical arm configuration and geometric parameters, establishing a kinematic model, comprises:

[0012] The DH kinematics model of the mechanical arm is established, the pose transformation relationship between adjacent links is decomposed into two rotations and two translations, and then the transformation matrix of the end of the mechanical arm relative to the base coordinate system is obtained by multiplication, including:

[0013]

[0014]

[0015]

[0016]

[0017]

[0018]

[0019]

[0020] wherein the joint and link parameters a i , α i , θ i , d i are DH parameters. The definitions and meanings of various parameters are as follows:

[0021] α i-1 : rotate the Z i-1 axis around the X i-1 axis by a certain angle so that the Z i-1 axis is parallel to the Z i axis, and the angle is defined as the link twist angle α i ;

[0022] a i-1 : move the Z i-1 axis along the X i-1 axis by a certain distance so that the Z i-1 axis coincides with the Z i axis, and the distance is defined as the link length a i-1 ;

[0023] θ i : rotate the X i-1 axis around the Z i axis by a certain angle so that the X i-1 axis is parallel to the X i axis, and the angle is defined as the joint angle θ i ;

[0024] d i : move the X i-1 axis along the Z i by a certain distance so that the X i-1 axis coincides with the X i axis, and the distance is defined as the link offset di .

[0025] s、c is the abbreviation of sin, cos.

[0026] Rot(X, a): rotate a angle around X axis;

[0027] Trans(a, 0, 0): translate a along X axis.

[0028] wherein, denotes the pose transformation matrix between adjacent links;

[0029] denote the pose description from the base to the end adjacent links, respectively;

[0030] denotes the description of the end of the robot arm relative to the base coordinate system;

[0031] r 11 , r 12 , r 13 , r 21 , r 22 , r 23 , r 31 , r 32 , r 33 , p x , p y , p z : correspond to the above six transformation matrices respectively, the results of multiplication, used to describe the position and attitude of the end relative to the base coordinate system.

[0032] Further, in step 2, the configuration space is initialized, and based on the established kinematics model, the joint angles corresponding to the starting state and the target state of the robot arm are solved as the starting node and the target node of the robot arm in the configuration space, including:

[0033] determine the starting pose and the target pose of the robot arm in the Cartesian space;

[0034] inverse the starting pose and the target pose of the robot arm to the configuration space through the established kinematics model, to obtain the corresponding starting joint angle and target joint angle as the starting node and the target node in the configuration space, and each node coordinate in the configuration space corresponds to six joint angle values of the robot arm.

[0035] Further, in step 3, the collision relationship between the robot arm and the obstacle in the Cartesian space is mapped to the configuration space by the Monte Carlo method, including:

[0036] The position of the obstacle in Cartesian space is determined. A large number of joint angle samples are randomly collected in Cartesian space using the Monte Carlo method, and the collision information between the robot arm and the obstacle is calculated. Then, the collision information is mapped to the robot arm configuration space.

[0037] Furthermore, in step 4, based on the configuration space and the starting node and target node of the robotic arm in the configuration space, node expansion is performed through a sampling strategy, and the parent node of the expanded new node is reselected and the region is reconnected. The node expansion is performed cyclically until the expanded new node enters the expansion range of the target node, generating a collision-free path, including:

[0038] Step A: Starting from the initial node q start Start by randomly sampling in the configuration space to obtain random nodes q rand Starting from the tree node q closest to the random node nearest Depart for q rand The direction is changed by one step λ to obtain the first new node q. new1 Determine if a collision has occurred on the current path. If no collision has occurred, save the node to the node tree; if a collision has occurred, discard q. new1 Resampling;

[0039] Step B: Determine the new node q new1 Do we need to enter the target node q? goal The expansion range, after entering the expansion range, directly connected to q new1 Complete the planning with the target node; if not entered, use q. new1 Using r as the center and r as the retrieval radius, the parent node of the new node is adjusted and the region is reconnected to complete the local optimization;

[0040] Step C: Calculate the new node q new1 The potential force at the point, moving along the direction of the potential force λ, generates another new node q. new2 ;

[0041] Step D: Determine another new node q new2 Do we need to enter the target node q? goal The expansion range, if it enters the expansion range, directly connected to q new2 Complete the planning with the target node; if not entered, use q. new2 Using r as the center and r as the radius, the parent node of the new node is adjusted and the region is reconnected to complete the local optimization;

[0042] Step E: Repeat steps BD until the newly generated node enters the target node q. goal The expansion range is calculated, and a series of node sequences from the starting node to the target node are output, resulting in a path from q. start to q goal Find the path points and complete the planning.

[0043] Further, the random node q rand is obtained by the following formula:

[0044]

[0045]

[0046] Wherein, P0 represents the initial bias rate, 0 min represents the minimum bias rate, 0 min <1, and P0>P min , iter, iter max represents the current iteration number and the set maximum iteration number.

[0047] Further, the sampling step size λ is obtained by the following formula:

[0048]

[0049] Wherein, iter is the current iteration number; d min is the target inflation range.

[0050] Further, the target node q goal inflation range determination method includes:

[0051] By calculating the Euclidean distance d i between the target node and the ith obstacle, subtracting the ith obstacle radius r i , taking the minimum value of (d i -r i ) to get the inflation range, when there are nodes to reach the target node q goal inflation range, directly connect the current node and the target node.

[0052] Further, the search radius r is calculated by the following formula:

[0053]

[0054] Wherein, n node represents the number of nodes in the current node tree.

[0055] Further, the potential field force at the new node q new1 includes:

[0056]

[0057]

[0058]

[0059] where F new1 represents the potential field attractive force, represents the potential field repulsive force, represents the potential field repulsive force;

[0060] F att (q) represents the potential field attractive force at q node, F rep (q) represents the potential field attractive force at q node, represents the gradient, and represents the attractive force scale factor, and represents the distance between the current node and the target node, represents the repulsive force scale factor, and represents the distance between the object and the obstacle, and represents the influence range of the obstacle. goal obs (q) represents the potential field attractive force at q node, represents the gradient, and represents the attractive force scale factor, and represents the distance between the current node and the target node, represents the repulsive force scale factor, and represents the distance between the object and the obstacle, and represents the influence range of the obstacle.

[0061] Compared with the prior art, the present application has the following beneficial effects:

[0062] 1. The present application expands the nodes by sampling strategy, and reselects the parent node and reconnects the region for the expanded new node. The node expansion is executed in a loop until the expanded new node enters the inflation range of the target node, generating a collision-free path, thereby minimizing unnecessary random search and improving planning efficiency. Compared with the traditional method, the present application can find a feasible path more quickly, thereby improving the motion efficiency of the robot arm in a complex scene. The introduction of this technology will greatly improve the applicability of the robot arm in various applications, providing higher efficiency and flexibility for automated workflow and robot application.

[0063] 2. The present application uses a highly intelligent path planning algorithm to minimize unnecessary random search and improve planning efficiency. Compared with the traditional method, the algorithm can find a feasible path more quickly, thereby improving the motion efficiency of the robot arm in a complex scene. The introduction of this technology will greatly improve the applicability of the robot arm in various applications, providing higher efficiency and flexibility for automated workflow and robot application.

[0064] 3. The present application proposes an innovative path planning technology to more effectively deal with obstacles in the robot arm workspace. The present application improves and perfects the node expansion method based on the traditional rapidly expanding random tree algorithm, and the specific method is to combine the artificial potential field method to propose a twice sampling expansion strategy.

[0065] 4. The present application inflates the target node to provide a faster judgment for completing path search.

[0066] 5. The present application uses dynamic target bias, adaptive step size and dynamic retrieval to provide adaptive search characteristics for the algorithm.

[0067] ​6. This method adopts a fusion of artificial potential field and fast expanding random tree algorithm. By constructing a gravitational potential field, performing secondary sampling, and expanding the target node, it guides the expansion of the node and generates a collision-free path. This can solve the technical problems of low planning efficiency and high path cost of traditional fast expanding random tree algorithm. Attached Figure Description

[0068] Figure 1 This is a flowchart of a robotic arm motion planning method according to an embodiment of the present invention;

[0069] Figure 2 This is a schematic diagram of the kinematic model of the robotic arm in an embodiment of the present invention;

[0070] Figure 3 This is a schematic diagram of target expansion in an embodiment of the present invention;

[0071] Figure 4 This is a schematic diagram of a single sampling method in an embodiment of the present invention;

[0072] Figure 5 This is a schematic diagram of the secondary sampling method in an embodiment of the present invention;

[0073] Figure 6 This is a dynamic retrieval radius variation curve in an embodiment of the present invention;

[0074] Figure 7 This is a schematic diagram illustrating the application of a three-dimensional environment in an embodiment of the present invention;

[0075] Figure 8 This is a schematic diagram illustrating the obstacle avoidance application of the robotic arm in a three-dimensional environment in an embodiment of the present invention. Detailed Implementation

[0076] The present invention will be further described below with reference to the accompanying drawings. The following embodiments are only used to more clearly illustrate the technical solution of the present invention, and should not be used to limit the scope of protection of the present invention.

[0077] Example 1:

[0078] like Figure 1 As shown, this embodiment provides a robotic arm motion planning method based on a configuration space sampling algorithm, including:

[0079] Step 1: Perform kinematic modeling based on the robotic arm's configuration and geometric parameters;

[0080] Step 2: Initialize the configuration space and solve for the joint angles corresponding to the poses of the robot arm in the initial and target states;

[0081] Step 3: Map the collision relationship between the robotic arm and obstacles in Cartesian space to configuration space using the Monte Carlo method;

[0082] Step 4, based on the configuration space, introduce secondary sampling, target bias, adaptive step and dynamic retrieval optimization strategy to improve the planning algorithm on the basis of traditional sampling algorithm, use the optimized algorithm for path planning and generate collision-free path.

[0083] Configuration space (also known as state space, general algorithm field called state space, mechanical arm in the planning state space is called configuration space): a mathematical model, including all possible state set, describes the possible state and their transition relationship in the problem. In this paper, the joint space of the robot arm is also called, which has 6 dimensions (θ1~θ6), each dimension corresponds to a joint angle of the 6-DOF robot arm.

[0084] Cartesian space: the three-dimensional space (X, Y, Z three dimensions) that can be seen directly.

[0085] As shown in Figure 2 , step 1, the DH kinematics model of the robot arm is established, which is to decompose the pose transformation relationship between adjacent links into two rotations and two translations, and then multiply to get the transformation matrix of the robot arm end relative to the base coordinate system, including:

[0086]

[0087]

[0088]

[0089]

[0090]

[0091]

[0092]

[0093]

[0094] Among them, represents the pose transformation matrix between adjacent links;

[0095] respectively represent the pose description between the base and the end adjacent links;

[0096] represents the description of the robot arm end relative to the base coordinate system;

[0097] r 11 , r 12 , r 13 , r 21 , r22 , r 23 , r 31 , r 32 , r 33 , p x , p y , p z correspond to the above six transformation matrices after multiplication, used to describe the position and attitude of the end relative to the base coordinate system;

[0098] In step 2, after determining the initial pose and target pose of the robot arm, the corresponding initial joint angle and target joint angle are obtained by the closed-form solution, as the initial node and target node in the configuration space. Each node coordinate in the configuration space corresponds to the value of the six joint angles of the robot arm.

[0099] In step 3, a large number of joint angle samples are randomly collected in the workspace by the Monte Carlo method and the collision detection relationship between the robot arm and the obstacle is calculated, and then the collision information is mapped to the robot arm configuration space.

[0100] In step 4, the specific content is as follows:

[0101] (1) As shown in Figure 3 , the first sampling is performed. Starting from q start , random nodes q rand are obtained in the configuration space by random sampling. From the tree node q nearest closest to the random node, the first new node q rand is obtained by taking a step length λ in the direction of q new1 . It is judged in the configuration space whether the current path has collided. If no collision occurs, the node is saved to the node tree. If a collision occurs, q new1 is discarded and resampled.

[0102] (2) As shown in Figure 5 , it is judged whether q new1 enters the target inflation range. After entering the inflation range, it is indicated that there is no obstacle in the range, and q new1 is directly connected to the target node, and the planning is completed. If it does not enter, the new node is adjusted with the parent node and reconnected with the region with q new1 as the center and r as the radius, and local optimization is completed:

[0103] (3) As shown in Figure 4 , the second sampling is performed. The potential field combined force after the calculation of the attractive force and the repulsive force at q new1 is calculated, and q new2 is generated by advancing λ in the direction of the potential field combined force.

[0104] (4) It is judged whether q new2Whether to enter the target expansion range, after entering the expansion range, it is indicated that there is no obstacle in the range, and the q is directly connected new2 with the target node, and the planning is completed; if not entered, the q is directly connected new2 with the target node, and the planning is completed; if not entered, the q is directly connected

[0105] (5) The above steps are cycled until the new node enters the target expansion range, the planning is completed, and the pseudo code of the algorithm is as follows:

[0106]

[0107] Further, in step 4, the random node q is calculated by the following formula: rand :

[0108]

[0109]

[0110] Wherein, P0 represents the initial bias (0 < P0 < 1), P min represents the minimum bias (0 < P min < 1), and P0 > P min , iter and iter max represent the current iteration number and the maximum iteration number set respectively;

[0111] Further, in step 4, the step length λ is calculated by the following formula:

[0112]

[0113] Wherein, iter is the current iteration number; d min is the target expansion range;

[0114] Further, as shown in Figure 5 , in step 4, the target expansion is obtained by calculating the Euclidean distance d i between the target node and all obstacles, subtracting the obstacle radius r i , and taking the minimum value of (d i -r i ), when a node reaches the range, the current node is directly connected with the target node, and the path planning is completed, and the pseudo code is as follows:

[0115]

[0116] Further, in step 4, the search radius r is calculated by the following formula, and the search radius change curve is shown in Figure 6 :

[0117]

[0118] where n node represents the number of nodes in the current node tree;

[0119] Further, in step 4, the potential field is defined by the following formula:

[0120] The potential field force at the new node q new1 is calculated, including:

[0121]

[0122]

[0123]

[0124]

[0125]

[0126] where F new1 represents the potential field force, represents the potential field attraction, represents the potential field repulsion;

[0127] F att (q) represents the potential field attraction at the q node, F rep (q) represents the potential field attraction at the q node, ∇ represents the gradient, the potential field attraction = the gradient of the potential field function; U att is the definition of the attraction field; U rep is the definition of the repulsion field;

[0128] where ξ represents the attraction scale factor, ρ(q, q goal ) represents the distance between the current node and the target node, η is the repulsion scale factor, ρ(q, q obs ) represents the distance between the object and the obstacle, and ρ0 represents the influence range of the obstacle.

[0129] Those skilled in the art will understand that the embodiments of the present application can be provided as methods, systems, or computer program products. Therefore, the present application can take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present application can take the form of a computer program product implemented on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROMs, optical storage, etc.) containing computer usable program code.

[0130] The computer program instructions can also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer-implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart block or blocks. Figure 1 one or more flow or blocks. Figure 1 one or more flow or blocks.

[0131] These computer program instructions can also be stored in a computer readable memory that can direct a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer readable memory produce an article of manufacture including instructions which implement the function specified in the flowchart block or blocks. Figure 1 one or more flow or blocks. Figure 1 one or more flow or blocks.

[0132] The computer program instructions can also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer-implemented process such that the instructions which execute on the computer or other programmable apparatus provide steps for implementing the functions specified in the flowchart block or blocks. Figure 1 one or more flow or blocks. Figure 1 one or more flow or blocks.

[0133] The above only is the preferred embodiment of the present application, it should be pointed out that, for those skilled in the technical field, without departing from the technical principles of the present application, can also make a number of improvements and variations, these improvements and variations should also be considered as the protection scope of the present application.

Claims

1. A method for motion planning of a robot arm based on a configuration space sampling algorithm, characterized in that, The method comprises the following steps: kinematics modeling according to the mechanical arm configuration and geometric parameters, establishing a kinematics model; initializing the configuration space, solving the joint angles corresponding to the starting state and the target state of the mechanical arm based on the established kinematics model, as the starting node and the target node of the mechanical arm in the configuration space; mapping the collision relationship between the mechanical arm and the obstacle in the Cartesian space to the configuration space through the Monte Carlo method; based on the configuration space and the starting node and the target node of the mechanical arm in the configuration space, expanding the nodes through a sampling strategy, and reselecting the parent nodes and reconnecting the regions of the expanded new nodes, and executing the node expansion in a loop until the expanded new nodes enter the inflation range of the target node, generating a collision-free path; based on the configuration space and the starting node and the target node of the mechanical arm in the configuration space, expanding the nodes through a sampling strategy, and reselecting the parent nodes and reconnecting the regions of the expanded new nodes, and executing the node expansion in a loop until the expanded new nodes enter the inflation range of the target node, generating a collision-free path, comprising: Step A: Starting from the initial node q start Start by randomly sampling in the configuration space to obtain random nodes q rand Starting from the tree node q closest to the random node nearest Depart for q rand The direction is changed by one step λ to obtain the first new node q. new1 Determine if a collision has occurred on the current path. If no collision has occurred, save the node to the node tree; if a collision has occurred, discard q. new1 Resampling; Step B: judge whether the new node q new1 enters the expansion range of the target node q goal , after entering the expansion range, directly connect q new1 with the target node, complete the planning; if not, adjust the parent node of q new1 with r as the search radius, and reconnect the region, complete the local optimization; Step C: Compute new node q new1 along the potential field joint force direction by λ to generate another new node q new2 ; Step D: judge whether another new node q new2 enters the target node q goal 's expansion range, if it enters the expansion range, directly connect q new2 with the target node, complete the planning; if it does not enter, adjust the parent node and reconnect the region to the new node with q new2 as the center and r as the radius, complete the local optimization; Step E: Repeat steps BD until the newly generated node enters the target node q. goal The expansion range is calculated, and a series of node sequences from the starting node to the target node are output, resulting in a path from q. start to q goal Find the path points and complete the planning; Compute new node q new1 Potential field joint forces at q, including: where F new1 represents a potential field attractive force, represents a potential field attractive force, represents a potential field repulsive force; F att (q) represents the potential gravitational force at node q, F rep (q) represents the potential repulsion at node q, ▽ represents calculating the gradient, ξ represents the gravitational scale factor, and ρ(q,q) represents the gravitational scale factor. goal ) represents the distance between the current node and the target node, η is the repulsion scale factor, and ρ(q,q) obs ) represents the distance between the object and the obstacle, and ρ0 represents the range of influence of the obstacle.

2. The method of claim 1, wherein, kinematics modeling according to the mechanical arm configuration and geometric parameters, establishing a kinematics model, comprising: establishing a DH kinematics model of the mechanical arm, decomposing the pose transformation relationship between adjacent linkages into twice rotation and twice translation motion, and then multiplying to obtain the transformation matrix of the mechanical arm end relative to the base coordinate system, comprising: wherein the joint and link parameters a i , a i , a i , d i are DH parameters; the definition and meaning of each parameter are as follows: alpha i-1 : rotate Z i-1 axis about X i-1 axis by an angle such that Z i-1 is parallel to Z i axis, this angle is defined as the link twist angle alpha i ; a i-1 : moving Z i-1 axis along X i-1 axis a distance such that Z i-1 coincides with Z i axis, this distance being defined as the link length a i-1 ; θ i : X i-1 axis is rotated around Z i axis by an angle such that X i-1 is parallel to X i axis, this angle is defined as joint angle θ i ; d i : X i-1 axis along the Z i direction by a distance such that X i-1 coincides with the X i axis, this distance being defined as the link offset d i ; s and c are the abbreviations of sin and cos; Rot(X, a): rotate around the X axis by a angle; Trans(a, 0, 0): translate along the X axis by a; wherein, denotes the pose transformation matrix between adjacent links; respectively denote the pose description between the base and the end adjacent link; represents a description of the end of the robot arm relative to the base coordinate system.

3. The method of claim 1, wherein, initializing the configuration space, solving the joint angles corresponding to the starting state and the target state of the mechanical arm based on the established kinematics model, as the starting node and the target node of the mechanical arm in the configuration space, comprising: determining the starting pose and the target pose of the mechanical arm in the Cartesian space; inverse solving the starting pose and the target pose of the mechanical arm to the configuration space through the established kinematics model to obtain the corresponding starting joint angle and target joint angle as the starting node and the target node in the configuration space, and each node coordinate in the configuration space corresponds to six joint angle values of the mechanical arm.

4. The method of claim 1, wherein, mapping the collision relationship between the mechanical arm and the obstacle in the Cartesian space to the configuration space through the Monte Carlo method, comprising: determining the position of the obstacle in the Cartesian space, randomly collecting a large number of joint angle samples in the Cartesian space through the Monte Carlo method and calculating the collision information between the mechanical arm and the obstacle, and then mapping the collision information to the mechanical arm configuration space.

5. The method of claim 1, wherein, The random node q rand Obtained by the equation: where P0represents an initial bias rate, 0 min represents a minimum bias rate, 0 min <1, and P0>P min iter, iter max represents the current iteration number and the set maximum iteration number.

6. The method of claim 1, wherein, The step length λ is obtained by the following formula: where iter is the current iteration number; d min is the target dilation range.

7. The method of claim 1, wherein, The target node q goal The determination method of the expansion range of the target node q includes: By calculating the Euclidean distance d between the target node and the ith obstacle i , subtracting the radius r of the ith obstacle i , taking the minimum value of (d i -r i ) to obtain the inflation range, when a node reaches the inflation range of the target node q goal , directly connecting the current node and the target node.

8. The method of Claim 1, wherein, The search radius r is calculated by the following formula: where n node represents the number of nodes in the current node tree.

Images