A robotic arm trajectory planning method combining a grasping network and grasping pose selection and adjustment

Abstract

The present invention belongs to the field of robotic motion planning, and discloses a robotic arm trajectory planning method combining a grasping network and grasping pose selection and adjustment, which includes the following steps: Step 1: Define the optimization problem of robotic arm trajectory planning and give a solution method; Step 2: Design a grasping pose selection strategy; Step 3: Design an unconstrained multi-objective optimization problem for grasping pose adjustment and give a solution method; Step 4: According to the methods provided in the above steps, design a robotic arm trajectory planning method combining grasping pose selection and adjustment. After generating a set of grasping poses for an object by the method of the grasping network, the present invention can, with the aid of the set of grasping poses, optimize the trajectory in a timely manner according to the scene, change and adjust the grasping pose, achieve the performance balance of smooth trajectory and collision-free trajectory, have a leading grasping planning success rate, and the approximate solution idea of grasping pose adjustment and the strategy of gradient preservation greatly reduce the computational cost, and the method has strong real-time performance.

Description

Technical Field

[0001] The present invention belongs to the field of robot motion planning, and particularly relates to a robotic arm trajectory planning method combining a grasping network and grasping pose selection and adjustment. Background Art

[0002] Robot grasping is an important research content in the field of robotics and can be widely applied to industrial manufacturing, home service, and national defense and military industries. However, compared with the structured traditional industrial environment, the uncertainties in the unstructured and complex environment pose great challenges to the motion planning of robot grasping.

[0003] Traditional grasping planning is usually the motion planning for a given target grasping pose. In complex scenarios, there may be stacking and occlusion between grasping targets, and the fixed grasping pose planning is likely to fail. The grasping strategies for unstructured environments mainly generate a set of possible grasping poses for the target scene using deep learning or grasping planning methods first, and then adopt a trajectory planning method to generate the motion trajectory of the robotic arm. However, this method of separate planning for grasping and motion is not continuous. In response to this problem, the literature "Manipulation planning with goal sets using constrained trajectory optimization" (A. Dragan et al., in IEEE International Conference on Robotics and Automation (ICRA), pp. 4582-4588, 2011) and the literature "Manipulation trajectory optimization with online grasp synthesis and selection" (L. Wang et al., in Robotics: Science and Systems (RSS), 2020) proposed a grasping motion planning strategy that comprehensively considers grasping pose selection and trajectory planning. However, the efficiency and success rate of the grasping pose selection strategy still need to be improved, and when generating grasping poses, the planning of the robotic arm is not considered. Direct planning on this set of grasping poses cannot guarantee the generation of a feasible grasping trajectory. Therefore, it is necessary to adjust the grasping poses according to the motion planning results. Summary of the Invention

[0004] The object of the present invention is to provide a robotic arm trajectory planning method combining a grasping network and grasping pose selection and adjustment to solve the above technical problems.

[0005] To solve the above technical problems, the specific technical solution of a robotic arm trajectory planning method combining a grasping network and grasping pose selection and adjustment of the present invention is as follows:

[0006] A robotic arm trajectory planning method combining a grasping network and grasping pose selection and adjustment includes the following steps:

[0007] Step 1: Define the optimization problem of robotic arm trajectory planning and give the solution method;

[0008] Step 2: Design the grasping pose selection strategy;

[0009] Step 3: Design an unconstrained multi-objective optimization problem for grasping pose adjustment and give the solution method;

[0010] Step 4: Design a robotic arm trajectory planning method combining grasping pose selection and adjustment according to the methods provided in the above steps.

[0011] Further, the specific steps of the said Step 1 include:

[0012] Define the optimization problem as:

[0013]

[0014] s.t.h(ξ,g)=0

[0015] where the trajectory ξ is defined as ξ: is a mapping from time [0,1] to joint poses, ξ * is the local optimal solution of the optimization problem, h(ξ,g) is the equality constraint function, and f traj (ξ) is the trajectory planning objective function, defined as:

[0016] f traj (ξ)=f obs (ξ)+λf smooth (ξ)

[0017] where λ is the weight of the smoothing objective function, and f smooth (ξ) is the smoothing objective function, defined as the sum of the squares of the velocities of the trajectory:

[0018]

[0019] f obs (ξ) is the obstacle avoidance function, defined as the sum of the collision costs of all points on the robotic arm along the trajectory:

[0020]

[0021] where is the set of points outside the robot, x: At the pose ξ(t) of the robotic arm at time t, a certain point on the robotic arm to the forward kinematic mapping of the task space; c(x(ξ(t),u)) represents the collision cost function of this point, which is used to penalize the points on the robotic arm that are close to or enter the obstacle;

[0022] The equality constraint is used to project the entire trajectory onto this constraint. Specifically:

[0023] h(ξ,g) = ξ(1) - g = 0

[0024] where ξ(1) is the last pose in the trajectory ξ, and g is the target pose currently selected from the grasping point set G; the solution method for the optimization problem of this trajectory planning is that at the i-th iteration, the trajectory is updated as follows until a feasible solution is found:

[0025]

[0026] where ξ i and ξ i+1 are the current trajectory and the updated trajectory respectively, is the gradient of the trajectory planning objective function at ξ i , is the update step size of the trajectory planning, A is the acceleration difference matrix of the trajectory, and the amount of each update is uniformly superimposed on the original trajectory, is the partial derivative of the equality constraint with respect to ξ at ξ i , b = h(ξ i ,g) is the value of the equality constraint function at ξ i .

[0027] Furthermore, the specific step 2 is divided into two stages:

[0028] In the first stage, the process at the i-th iteration is:

[0029] Step 1 performs an unconstrained update of the trajectory planning:

[0030]

[0031] ξ i ' +1 is the trajectory after the unconstrained update of the trajectory planning;

[0032] Step 2 selects the first k grasping poses g j ∈G (j = 1,..., k) that are closest to the end pose of the current trajectory, and calculates the cost function values c(g j ):

[0033] c(g j ) = f obs (gj ) + λ||g j - ξ(1)||

[0034] Among them, f obs (g j ) is the obstacle avoidance function value at g j ;

[0035] Step 3 Select the grasping pose with the minimum cost as the new end pose of the trajectory;

[0036] In the second stage, in the process of the i-th iteration, Step 1 and Step 3 are exactly the same as those in the first stage, and Step 2 becomes:

[0037] Select the first k poses g j ∈ G (j = 1,..., k) that are closest to the current end pose of the trajectory, and calculate the second-stage cost function values c'(g j ) of these poses:

[0038]

[0039] is the interpolation trajectory from the i-th point of the trajectory to g i , is the trajectory planning objective function value of

[0040] Furthermore, the said Step 3 includes the following specific steps:

[0041] The optimization problem of grasping pose adjustment is defined as follows:

[0042]

[0043] Among them, g * is the locally optimal grasping pose solved, f(ξ) is the objective function of the grasping pose adjustment optimization problem, defined as f(ξ) = f obs (ξ) + λf smooth (ξ) + μf goal (ξ), μ is the weight of the grasping pose adjustment objective function, f goal (ξ) is the grasping pose adjustment objective function, defined as follows:

[0044] f goal (ξ) = ||W(FK(ξ(1)) - FK(g))|| 2 - logg(ξ)

[0045] Among them, FK(·) is the forward kinematic mapping from the pose of the robotic arm to the coordinates of the end effector in the task space. W = diag(w1,..., w6) is the weight matrix, and the degrees of freedom that can rotate freely are set to 0. g(ξ) is defined as:

[0046]

[0047] φ is the maximum rotation angle limit, The degrees of freedom that can rotate freely are set to 1, and the rest are 0;

[0048] The solution method for the optimization problem of the grasping pose adjustment is to give an idea of approximate solution. At the i-th iteration, the following update is performed and used as the new target pose:

[0049]

[0050] Among them, g i and g i+1 are the current target pose and the new target pose respectively. ξ i (1) is the last pose in the current trajectory, κ is the update step size of the grasping pose adjustment, and TracIK(·) represents finding the inverse kinematic solution closest to the joint space distance of ξ i (1), is the trajectory after the grasping pose adjustment update, and is the last pose among them, The solution method of

[0051]

[0052] Among them, is the gradient of the t-th weighted objective function in f(ξ) (t = 1, 2, 3), and α t is The weight value of t is solved through the following process:

[0053] Step 1 Let α = (α1, α2, α3) and initialize it to

[0054] Step 2 Calculate the intermediate variable matrix M ∈ R 3×3 whose element value in the a-th row and b-th column is

[0055] Step 3 Calculate an intermediate variable and solve the following optimization problem:

[0056]

[0057] The solution method is:

[0058]

[0059] Among them,

[0060] Step 4 Update the α value to Among them, is a 3D row vector with the item being 1 and the rest being 0,

[0061] Repeat Step 3 and Step 4 until the maximum number of iterations is reached or the calculation result of is 0.

[0062] Furthermore, the said Step 4 includes the following specific steps:

[0063] Step 1 Initialization, initial trajectory ξ0, grasping point set G, initial target pose g0;

[0064] Step 2 In the i-th iteration, according to Step 2, select a new grasping pose g i , and save

[0065] Step 3 According to Step 3, use the saved to obtain a new target pose g i+1 after adjusting the grasping pose, perform a collision detection on this target pose. If there is no collision, keep it. If there is a collision, let g i+1 = g i ;

[0066] Step 4 The current equality constraint becomes h(ξ, g i+1 ) = 0. According to Step 1, use the saved to obtain the updated

[0067] trajectory ξ i+1 ;

[0068] Step 5 Repeat Step 2 - 4. If ξ i+1 has no collision and can reach the target pose in the i-th iteration, then ξ i+1 is the final solution of the method.

[0069] A robotic arm trajectory planning method combining a grasping network and grasping pose selection and adjustment of the present invention has the following advantages: The present invention provides a robotic arm motion planning method that comprehensively considers trajectory optimization, grasping pose selection, and adjustment. After generating a set of grasping poses for an object by means of the grasping network, it can, with the aid of the set of grasping poses, optimize the trajectory in a timely manner according to the scenario, change and adjust the grasping pose, achieving a performance balance between trajectory smoothness and trajectory collision avoidance, having a leading grasping planning success rate, and the approximate solution idea for grasping pose adjustment and the strategy of gradient preservation greatly reduce the computational cost, and the method has strong real-time performance. BRIEF DESCRIPTION OF THE DRAWINGS

[0070] Figure 1 is a schematic diagram of the grasping pose selection strategy proposed by the present invention;

[0071] Figure 2 is a grasping simulation result diagram including intermediate processes in a desktop object scenario and schematically shows a single iteration process. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0072] In order to better understand the purpose, structure, and function of the present invention, the following further describes in detail a robotic arm trajectory planning method combining a grasping network and grasping pose selection and adjustment of the present invention with reference to the accompanying drawings.

[0073] As Figure 2 shown, a robotic arm trajectory planning method combining a grasping network and grasping pose selection and adjustment of the present invention includes the following steps:

[0074] Step 1: Define the optimization problem of robotic arm trajectory planning and give a solution method. The optimization problem is defined as:

[0075]

[0076] s.t. h(ξ, g) = 0

[0077] where the trajectory ξ is defined as ξ: is a mapping from time [0, 1] to joint poses, ξ * is a local optimal solution of the optimization problem, h(ξ, g) is an equality constraint function, f traj (ξ) is the trajectory planning objective function, which is defined as:

[0078] f traj (ξ) = f obs (ξ) + λf smooth (ξ)

[0079] where λ is the weight of the smoothing objective function, and f smooth (ξ) is the smoothing objective function, which is defined as the sum of the squares of the velocities of the trajectory:

[0080]

[0081] f obs $f(\xi)$ is the obstacle avoidance function, defined as the sum of the collision costs of all points on the robotic arm along the trajectory:

[0082]

[0083] where, is the set of external points of the robot, $x$: represents a certain point on the robotic arm at the pose $\xi(t)$ of the robotic arm at time $t$ to the forward kinematic mapping of the task space; $c(x(\xi(t), u))$ represents the collision cost function of this point, which is used to penalize the points on the robotic arm that are close to or enter the obstacle.

[0084] The equality constraint is used to project the entire trajectory onto this constraint, specifically:

[0085] $h(\xi, g)=\xi(1)-g = 0$

[0086] where, $\xi(1)$ is the last pose in the trajectory $\xi$, and $g$ is the target pose currently selected from the grasping point set $G$.

[0087] The solution method for the optimization problem of this trajectory planning is that at the $i$-th iteration, the trajectory is updated as follows until a feasible solution is found:

[0088]

[0089] where, $\xi$ i and $\xi$ i+1 are the current trajectory and the updated trajectory respectively, is the gradient of the trajectory planning objective function at $\xi$ i , is the update step size of the trajectory planning, $A$ is the acceleration difference matrix of the trajectory, and the amount of each update is evenly superimposed on the original trajectory, is the partial derivative of the equality constraint with respect to $\xi$ at $\xi$ i , $b = h(\xi$ i , $g)$ is the value of the equality constraint function at $\xi$ i .

[0090] Step 2: As Figure 1 shown, design the grasping pose selection strategy, which is specifically divided into two stages.

[0091] In the first stage, the process at the $i$-th iteration is:

[0092] Step 1 Make an unconstrained update of the trajectory planning:

[0093]

[0094] ξ i ' +1 is the trajectory after the unconstrained update of the trajectory planning.

[0095] Step 2 Select the top k grasping poses g that are closest to the end pose of the current trajectory j ∈G (j = 1, ..., k), and calculate the cost function values c(g j ):

[0096] c(g j ) = f obs (g j ) + λ||g j - ξ(1)||

[0097] where f obs (g j ) is the obstacle avoidance function value at g j .

[0098] Step 3 Select the grasping pose with the minimum cost as the new end pose of the trajectory.

[0099] In the second stage, in the process of the i-th iteration, Step 1 and Step 3 are exactly the same as in the first stage, and Step 2 becomes:

[0100] Select the top k poses g that are closest to the end pose of the current trajectory j ∈G (j = 1, ..., k), and calculate the cost function values c'(g j ) in the second stage:

[0101]

[0102] is the interpolation trajectory from the i-th point of the trajectory to g i . is the trajectory planning objective function value of.

[0103] Step 3: Design an unconstrained multi-objective optimization problem for the grasping pose adjustment and give a solution method. The optimization problem of the grasping pose adjustment is defined as follows:

[0104]

[0105] where g * is the locally optimal grasping pose obtained by solving, and f(ξ) is the objective function of the optimization problem of the grasping pose adjustment, defined as f(ξ) = f obs (ξ) + λf smooth (ξ) + μf goal(ξ), μ is the weight of the grasping pose adjustment objective function, f goal (ξ) is the grasping pose adjustment objective function, defined as follows:

[0106] f goal (ξ) = ||W(FK(ξ(1)) - FK(g))|| 2 -logg(ξ)

[0107] Among them, FK(·) is the forward kinematic mapping from the manipulator pose to the coordinates of the end effector in the task space, W = diag(w1,..., w6) is the weight matrix, the degrees of freedom that can rotate freely are set to 0, and g(ξ) is defined as:

[0108]

[0109] φ is the maximum rotation angle limit, The degrees of freedom that can rotate freely are set to 1, and the rest are 0.

[0110] The solution method for the optimization problem of this grasping pose adjustment is to give an approximate solution idea. At the i-th iteration, the following update is performed and used as the new target pose:

[0111]

[0112] Among them, g i and g i+1 are the current target pose and the new target pose respectively, ξ i (1) is the last pose in the current trajectory, κ is the grasping pose adjustment update step size, TracIK(·) represents obtaining the inverse kinematic solution closest to the joint space distance of ξ i (1), is the trajectory after the grasping pose adjustment update, and is the last pose among them, The solution method of is:

[0113]

[0114] Among them, is the gradient of the t-th weighted objective function in f(ξ) (t = 1, 2, 3), α t is 's weight value, α t is solved through the following process:

[0115] Step 1 Let α = (α1, α2, α3) and initialize it to

[0116] Step 2 Calculate the intermediate variable matrix M ∈ R 3×3, the element value of its \(a\) -th row and \(b\) -th column is

[0117] Step 3 Calculate an intermediate variable and solve the following optimization problem:

[0118]

[0119] The solution method is:

[0120]

[0121] where,

[0122] Step 4 Update the α value to where, is a 3 - dimensional row vector with the \( \) -th item being 1 and the rest being 0. Repeat Step 3 and Step 4 until the maximum number of iterations is reached or the calculation result of \( \) is 0.

[0123] Step 4: According to the method provided in the above steps, design the robotic arm trajectory planning method that combines grasping pose selection and adjustment as follows:

[0124] Step 1 Initialize, the initial trajectory \(\xi_0\), the grasping point set \(G\), and the initial target pose \(g_0\);

[0125] Step 2 In the \(i\) -th iteration, according to Step 2, select a new grasping pose \(g\) i , and save

[0126] Step 3 According to Step 3, use the saved to obtain the new target pose \(g\) i+1 after grasping pose adjustment. Perform a collision detection on this target pose. If there is no collision, retain it; if there is a collision, let \(g\) i+1 = \(g\) i ;

[0127] Step 4 The current equality constraint becomes \(h(\xi, g\) i+1 ) = 0. According to Step 1, use the saved to obtain the updated trajectory \(\xi\) i+1 ;

[0128] Step 5 Repeat Step 2 - 4. If in the \(i\) -th iteration \(\xi\) i+1 has no collision and can reach the target pose, then \(\xi\) i+1 is the final solution of the method.

[0129] Example:

[0130] The implementation technical solution of the present invention is as follows:

[0131] 1) Define the optimization problem of robotic arm trajectory planning and derive the solution method:

[0132] Define the optimization problem as:

[0133]

[0134] s.t. h(ξ, g) = 0

[0135] where, f traj (ξ) = f obs (ξ) + λf smooth (ξ), and take λ = 0.1.

[0136] In specific applications, discretize the trajectory: q1 ~ q n are the trajectory poses at the 1st to nth moments. Assume the time interval between poses after discretization is Δt, and define q0 as the initial pose. For a 7-degree-of-freedom robotic arm, d = 7. The smoothing objective function is written as:

[0137]

[0138] In the obstacle avoidance function, u i represents the points inside it after discretization. The obstacle avoidance function is written as:

[0139]

[0140] The equality constraint is specifically as follows in implementation:

[0141] h(ξ, g) = ξ(1) - g = 0

[0142] At this time, ξ(1) = q n , and for convenience, it is still expressed in this form.

[0143] The solution method for the optimization problem of this trajectory planning is that at the i-th iteration, the trajectory is updated as follows until a feasible solution is found:

[0144]

[0145] where, ξ i and ξ i+1 are the current trajectory and the updated trajectory respectively, is the gradient of the trajectory planning objective function at ξ i , is the update step size of the trajectory planning, take A is the acceleration difference matrix of the trajectory, and the amount of each update is uniformly superimposed on the original trajectory, is the partial derivative of the equality constraint with respect to ξ at ξ i , and b = h(ξ i , g) is the value of the equality constraint function at ξ i .

[0146] 2) Design a grasping pose selection strategy, which is specifically divided into two stages.

[0147] The first 10 iterations are the first stage. The process at the i-th iteration is as follows:

[0148] (1) Perform an unconstrained update of the trajectory planning:

[0149]

[0150] ξ′ i+1 is the trajectory after the unconstrained update of the trajectory planning.

[0151] (2) Select the first k grasping poses g j ∈ G (j = 1,..., k), take k = 5, and calculate the cost function values c(g j ) of these poses:

[0152] c(g j ) = f obs (g j ) + λ|g j - ξ(1)||

[0153] where f obs (g j ) is the obstacle avoidance function value at g j .

[0154] (3) Select the grasping pose with the minimum cost as the new trajectory end pose.

[0155] The 11th to 30th iterations are the second stage. The process at the i-th iteration is as follows:

[0156] Step 1 Perform an unconstrained update of the trajectory planning:

[0157]

[0158] ξ′ i+1 is the trajectory after the unconstrained update of the trajectory planning.

[0159] Step 2 Select the first k grasping poses g j ∈ G (j = 1,..., k), take k = 5, and calculate the cost function values c(g j ) of these poses:

[0160]

[0161] is the interpolated trajectory from the i-th point of the trajectory to g i of the trajectory. is the value of the trajectory planning objective function of.

[0162] Step 3 Select the grasping pose with the minimum cost as the new end pose of the trajectory.

[0163] 3) Design an unconstrained multi-objective optimization problem for grasping pose adjustment and give a solution method. The optimization problem of grasping pose adjustment is defined as follows:

[0164]

[0165] where g * is the locally optimal grasping pose solved, f(ξ) is the objective function of the optimization problem of grasping pose adjustment, defined as f(ξ) = f obs (ξ) + λf smooth (ξ) + μf goal (ξ), μ is the weight of the objective function of grasping pose adjustment, set as μ = 3, f goal (ξ) is the objective function of grasping pose adjustment, defined as follows:

[0166] f goal (ξ) = ||W(FK(ξ(1)) - FK(g))|| 2 - logg(ξ)

[0167] where FK(·) is the forward kinematic mapping from the manipulator pose to the coordinates of the end effector in the task space, W is the weight matrix, set as W = diag(1, 1, 1, 1, 0, 1), the degree of freedom that can rotate freely is the rotation around the y-axis of the end effector coordinates, and g(ξ) is defined as:

[0168]

[0169] φ is the maximum rotation angle limit, set as φ = 40°, the degree of freedom that can rotate freely is set as 1, and the rest are 0.

[0170] The solution method for the optimization problem of this grasping pose adjustment is to give an approximate solution idea. At the i-th iteration, the following update is made and used as the new target pose:

[0171] g i+1 = TracIK(FK(ξ i (1)) + Δθ)

[0172] where g i and gi+1 are the current target pose and the new target pose, ξ i (1) is the last pose in the current trajectory, and TracIK(·) represents obtaining the distance ξ i (1) the inverse kinematic solution closest to the joint space distance, and Δθ is a non-linear function defined according to experience Take κ = 8, is the trajectory after the grasping pose adjustment and update, while is the last pose among them, The solution method of is:

[0173]

[0174] Among them, is the gradient of the t-th weighted objective function in f(ξ) (t = 1, 2, 3), α t is the weight value of, α t is solved through the following process:

[0175] Step 1 Let α = (α1, α2, α3), and initialize it to

[0176] Step 2 Calculate the intermediate variable matrix M ∈ R 3×3 , and the element value of its a-th row and b-th column is

[0177] Step 3 Calculate an intermediate variable and solve the following optimization problem:

[0178]

[0179] The solution method is:

[0180]

[0181] Among them,

[0182] Step 4 Update the α value to Among them, is a 3D row vector with 1 for the -th term and 0 for the rest of the terms, repeat Step 3 and Step 4 until the maximum number of iterations is reached or the calculation result of is 0.

[0183] 4) According to the method provided in the above steps, the process of designing a robotic arm trajectory planning method that combines grasping pose selection and adjustment is as follows:

[0184] Step 1 Initialization. After the grasping point set G is generated by the graspnet-1billion network, inverse kinematics solution is performed, and the poses that collide with the environment are removed. q0 = [0.0, -1.285, 0, -2.356, 0.0, 1.571, 0.785] T , the initial target pose The initial trajectory ξ0 is a cubic spline interpolation from q0 to g0;

[0185] Step 2 In the i-th iteration, according to 2), select a new grasping pose g i , and save it

[0186] Step 3 According to 3), use the saved to obtain the new target pose g after adjusting the grasping pose i+1 . Perform a collision detection on this target pose. If there is no collision, keep it. If there is a collision, let g i+1 = g i ;

[0187] Step 4 The current equality constraint becomes h(ξ, g i+1 ) = 0. According to 1), use the saved to obtain the updated trajectory ξ i+1 ;

[0188] Step 5 Repeat Step 2 - 4. If in the i-th iteration ξ i+1 has no collision and can reach the target pose, then ξ i+1 is the final solution of the method.

[0189] Step 6 Write the obtained trajectory into the control code of the robotic arm to control the robotic arm to complete the grasping.

[0190] As Figure 2 shown is the grasping simulation result diagram including the intermediate process in the desktop object scenario.

[0191] It can be understood that the present invention is described through some embodiments. Those skilled in the art know that without departing from the spirit and scope of the present invention, various changes or equivalent replacements can be made to these features and embodiments. Additionally, under the teaching of the present invention, these features and embodiments can be modified to adapt to specific situations and materials without departing from the spirit and scope of the present invention. Therefore, the present invention is not limited by the specific embodiments disclosed herein, and all embodiments falling within the scope of the claims of this application belong to the scope protected by the present invention.

Claims

1. A robotic arm trajectory planning method combining a grasping network and grasping pose selection and adjustment, characterized in that, It includes the following steps: Step 1: Define the optimization problem of robotic arm trajectory planning and give the solution method; The optimization problem is defined as: s.t.h(ξ,g)=0 where the trajectory ξ is defined as ξ: is a mapping from time [0, 1] to joint poses, ξ * is a local optimal solution of the optimization problem, h(ξ, g) is the equality constraint function, and f traj (ξ) is the trajectory planning objective function, which is defined as: f traj f(ξ) = f obs f(ξ) + λf smooth (ξ) where λ is the weight of the smoothing objective function, and f smooth (ξ) is the smoothing objective function, defined as the sum of the squares of the velocities of the trajectories: f obs (ξ) is the obstacle avoidance function, defined as the sum of the collision costs of all points on the robotic arm along the trajectory: Among them, is the set of external points of the robot, and x(ξ(t), u) refers to a point on the robotic arm at the joint pose at time t to the mapping of the coordinates in the task space; c(x(ξ(t), u)) represents the collision cost function of this point, which is used to penalize the points on the robotic arm that are close to or enter the obstacle; The equality constraint is used to project the entire trajectory onto this constraint. Specifically: h(ξ,g)=ξ(1)-g=0 Where ξ(1) is the last pose in the trajectory ξ, and g is the target pose currently selected from the grasping point set G; The solution method for the optimization problem of this trajectory planning is that at the i-th iteration, the trajectory is updated as follows until a feasible solution is found: Among them, ξ i and ξ i+1 are the current trajectory and the updated trajectory respectively, is the gradient of the trajectory planning objective function at ξ i , is the update step size of the trajectory planning, A is the acceleration difference matrix of the trajectory, and the amount of each update is uniformly superimposed on the original trajectory, is the partial derivative of the equality constraint with respect to ξ at ξ i , b = h(ξ i , g) is the value of the equality constraint function at ξ i ; Step 2: Design the grasping pose selection strategy; The specific Step 2 is divided into two stages: In the first stage, the process at the i-th iteration is: Step 1 makes an unconstrained update of the trajectory planning: is the trajectory after the unconstrained update of the trajectory planning; Step 2 Select the top k grasping poses \(g\) that are closest to the end - pose of the current trajectory j \(\in G\), \(j = 1,\cdots,k\), calculate the cost - function values \(c(g\) j ): c(g j ) = f obs (g j ) + λ||g j - ξ(1)|| where f obs (g j ) is the obstacle avoidance function value at g j ; Step 3 selects the grasping pose with the minimum cost as the new end pose of the trajectory; In the second stage, in the process at the i-th iteration, Step 1 and Step 3 are exactly the same as those in the first stage, and Step 2 becomes: Select the first k poses \(g\) that are closest to the pose at the end of the current trajectory j \(\in G\), \(j = 1,\cdots,k\), calculate the values of the second-stage cost function \(c'(g\) j ): is the interpolated trajectory from the i-th point of the trajectory to g i of the trajectory, is the trajectory planning objective function value; Step 3: Design an unconstrained multi-objective optimization problem for grasping pose adjustment and give the solution method; The specific steps of Step 3 are as follows: The optimization problem of grasping pose adjustment is defined as follows: Among them, g * To solve the locally optimal grasping pose, f(ξ) is the objective function of the optimization problem for grasping pose adjustment, defined as f(ξ) = f obs (ξ) + λf smooth (ξ) + μf goal (ξ), where μ is the weight of the objective function for grasping pose adjustment, and f goal (ξ) is the objective function for grasping pose adjustment, defined as follows: f goal f(ξ) = ||W(FK(ξ(1)) - FK(g))|| 2 -log g(ξ) Among them, FK(·) is the forward kinematic mapping from the pose of the robotic arm to the coordinates of the end effector in the task space, W = diag(w1,..., w6) is the weight matrix, and the degrees of freedom that can rotate freely are set to 0. g(ξ) is defined as: φ is the maximum rotation angle limit, The degree of freedom that can rotate freely is set to 1, and the rest are set to 0; The solution method for the optimization problem of this grasping pose adjustment is to give an approximate solution idea. At the i-th iteration, update as follows and use it as the new target pose: where, g i and g i+1 are the current target pose and the new target pose respectively, ξ i (1) is the last pose in the current trajectory, κ is the grasping pose adjustment update step size, and TracIK(·) represents obtaining the inverse kinematic solution with the closest joint space distance to ξ i (1), is the trajectory after the grasping pose adjustment update, and is the last pose among them, The solution method of is as follows: Among them, is the gradient of the t-th weighted objective function in f(ξ), where t = 1, 2, 3, and α t is the weight value of, α t is solved through the following process: Step 1 Let α = (α1, α2, α3) and initialize it as Step 2 Calculate the intermediate variable matrix M ∈ R 3×3 , where the element value in the a-th row and b-th column is Step 3 Calculate an intermediate variable and solve the following optimization problem: The solution method is: Among them, Step 4 Update α value to in, for 3D row vector with 1 as one and 0 as the other. Repeat Step 3 and Step 4 until the maximum number of iterations is reached or The calculation result is 0; Step 4: According to the method provided in the above steps, design a robotic arm trajectory planning method that combines grasping pose selection and adjustment.

2. The robotic arm trajectory planning method combining a grasping network and grasping pose selection and adjustment according to claim 1, characterized in that, The specific steps of Step 4 are as follows: Step 1 Initialization, initial trajectory ξ0, grasping point set G, initial target pose g0; Step 2 In the i-th iteration, according to Step 2, select a new grasping pose g i , and save it Step 3 According to Step 3, using the saved to obtain the new target pose g after the grasping pose adjustment i+1 , perform a collision detection on this target pose. If there is no collision, retain it. If there is a collision, set g i+1 = g i ; Step 4 The current equality constraint becomes h(ξ, g i+1 ) = 0. According to Step 1, using the saved to obtain the updated trajectory ξ i+1 ; Step 5 Repeat Steps 2 - 4. If there is no collision and the target pose can be reached in the $i$-th iteration, then $\xi$ i+1 is the final solution of the method. i+1 ​

Images