A robot arm trajectory planning method and device for grabbing moving objects on a conveyor belt

Abstract

The present invention discloses a robot arm trajectory planning method and device for grabbing moving objects on a conveyor belt. By defining the optimization problem of grabbing dynamic objects on a conveyor belt and the robot arm trajectory representation method, the pre-grabbing trajectory end constraint, the robot arm joint kinematic constraint, and the robot arm collision constraint are transformed, and then the gradient of the transformed total objective function is calculated, and the optimization solver is used to solve it, and the trajectory is input into a controller to control the robot arm to grab the object. The present invention can plan a trajectory for grabbing a uniformly moving object on a conveyor belt within a limited time according to the position and speed of the object on the conveyor belt input in real time. The trajectory has the local optimal property of balanced trajectory duration and smoothness, which can enable the robot arm to quickly approach the dynamic object and accurately grab it, and it meets the constraints of joint kinematics and obstacle avoidance. The present invention has a leading success rate and can perform fast and accurate grabbing at a higher conveyor belt movement speed.

Description

Technical Field

[0001] The invention belongs to the field of robot motion planning, and in particular relates to a robot arm trajectory planning method and device for grabbing a moving object on a conveyor belt. Background Art

[0002] Conveyor belts can transport large quantities of items over short distances, effectively saving manpower for handling. The use of robotic arms to automatically grab and place objects on conveyor belts is in high demand in industries such as industry, warehousing, and logistics sorting. However, most robotic arm grabbing technologies used in conveyor belt scenarios can only grab objects when the conveyor belt stops or runs at a very low speed, and fail to consider the optimal time of the robotic arm trajectory, which greatly hinders the efficiency of production and transportation.

[0003] Traditional robotic arm grasping methods only perform motion planning for fixed targets, which cannot meet the problem of time and space constraints for grasping dynamic objects, that is, reaching the correct position where the dynamic object is currently located within a certain time for grasping. In response to this problem, the document "Provably constant-time planning and replanning for real-time grasping objects off a conveyor belt" (F. Islam et al., The International Journal of Robotics Research, 2021, 40 (12-14), 1370-1384) proposes a method for generating a fixed-time robotic arm trajectory, and the trajectory conforms to the kinematic constraints of the robotic arm, so that the grasping of moving objects on the conveyor belt can be completed. However, the current method does not consider the time optimality of the trajectory, and cannot achieve fast and accurate grasping at higher conveyor belt movement speeds. The rapid grasping of dynamic objects places high technical requirements on the robotic arm trajectory planning, which not only considers the optimal trajectory time, but also needs to meet the robotic arm joint constraints and avoid obstacles. At the same time, it must also meet the trajectory end conditions of reaching a specific position at a specific time to grasp the moving object. Summary of the invention

[0004] In order to solve the shortcomings of the prior art and achieve the purpose of the robot arm safely and quickly grabbing the moving object on the conveyor belt, the present invention adopts the following technical solution:

[0005] A robot arm trajectory planning method for grabbing a moving object on a conveyor belt comprises the following steps:

[0006] Step 1: Define the grasping of dynamic objects on the conveyor belt and the robot arm trajectory representation; define the grasping of uniformly moving objects on the conveyor belt by the robot arm as the pre-grasping trajectory and the grasping trajectory. The pre-grasping trajectory is the trajectory from the initial position of the robot arm to the pre-grasping position, and the grasping trajectory is the trajectory from the pre-grasping position to the grasping position.

[0007] Step 2: Convert the end constraints of the pre-grasping trajectory; convert the end pose constraints of the pre-grasping trajectory into the end pose objective function, based on the constraints of the key points of the end trajectory in Cartesian space, and obtain the end velocity constraints of the pre-grasping trajectory after inverse kinematics calculation;

[0008] Step 3: Convert the kinematic constraints and collision constraints of the robot joints; based on the pre-grasping trajectory of each joint and the maximum and minimum joint poses of the robot corresponding to the robot, convert the robot joint pose constraints of the pre-grasping trajectory; based on the trajectory speed of each joint and the maximum joint speed of the robot corresponding to the robot, convert the robot joint speed constraints of the pre-grasping trajectory; based on the trajectory acceleration of each joint and the maximum joint acceleration of the robot corresponding to the robot, convert the robot joint acceleration constraints of the pre-grasping trajectory; based on the pre-grasping trajectory and the upper surface height of the grasping target object, convert the obstacle avoidance constraint;

[0009] Step 4: Based on the transformed constraints, construct the total objective function of the trajectory and calculate the gradient, obtain the gradient of the total objective function with respect to the trajectory key points and trajectory duration vector, use the optimization solver to solve the gradient, and input the obtained trajectory into the controller to control the robotic arm to grasp the target object.

[0010] Furthermore, the step 1 includes the following specific steps:

[0011] First, the representation method and planning problem of the pre-grasping trajectory are defined. The pre-grasping trajectory is defined as the trajectory from the initial position of the robot to the pre-grasping position. The pre-grasping position is defined as the inverse kinematics of the pre-grasping position of the end effector of the robot moving along the negative direction of the approach vector for a distance h. For a serial robot with m degrees of freedom, the pre-grasping trajectory is represented by ξ(t), which is the time from time t to the robot joint space. The mapping of has a total trajectory duration T, is a piecewise function with the number of trajectory segments M, and its i-th trajectory ξ i (t) is defined as follows:

[0012]

[0013] where β(t)=(1,t,...,t 2s-1 ) T is the basis of the trajectory curve, is the coefficient matrix of the i-th trajectory, s is the order of the specified derivative value of the trajectory, t i-1and t i is the minimum and maximum time limit of the trajectory, and the i-th trajectory has a duration of T i =t i -t i-1 ; For the M segments of pre-captured trajectories, the duration of all trajectories constitutes the trajectory duration vector T = (T 1 ,...,T M ) T , the coefficient matrices of all segment trajectories constitute the trajectory coefficient matrix

[0014] The problem of pre-grasp trajectory planning is defined as solving the following optimization problem:

[0015]

[0016] stT>0

[0017] ξ(0)=q 0

[0018]

[0019] x (1) (ξ(T))=v c +v ⊥

[0020] ξ (2) (T) = 0

[0021]

[0022]

[0023]

[0024]

[0025] Where q=(q 1 ,...,q M ) is the key point of the trajectory, which contains the one-to-one corresponding robot arm posture in the trajectory time vector T. is the initial objective function, ρ is the time optimization weight, is the initial position of the robot arm, It is a positive kinematic mapping, which maps from joint space to Cartesian space is the conveyor belt speed is the initial grasping posture, i.e. the grasping posture at the initial time t = 0, is the grasping posture approach vector, is the unitized grasping posture approach vector, is the collision-free joint space; q max,q min are the maximum and minimum joint positions of the robot arm, v max ,a max are the joint speed limit and joint acceleration limit, ξ (1) (t),ξ (2) (t),ξ (3) (t) are trajectory velocity, trajectory acceleration and trajectory jerk, respectively;

[0026] Then, define the grasping trajectory and its generation method; the grasping trajectory is the trajectory from the pre-grasping posture to the grasping posture, and the generation method is:

[0027] Given the current position q of the robot arm j , the robot arm speed at the next moment is:

[0028]

[0029] Among them, J + is the pseudo-inverse of the Jacobian matrix of the robot arm. During this process, the robot arm gripper gradually closes until the robot arm reaches the grasping position, and the grasping action phase ends.

[0030] Furthermore, step 2 is specifically divided into two stages:

[0031] The trajectory end pose constraint is converted into adding the end pose objective function The definition is as follows:

[0032]

[0033] Among them, ξ(T) represents the pre-grasp trajectory function value when time t is the total trajectory length T, q M Pre-capture for the current iteration

[0034] Take the pose, ∈ is a small value, μ is a definable parameter, and the function φ μ [·] is defined as:

[0035]

[0036] Among them, q M Solved by:

[0037]

[0038] The terminal velocity constraint of the pre-grasp trajectory is transformed into:

[0039] ξ (1) (T) = J + (q M )(v c +v ⊥ ).

[0040] Furthermore, in step 3, the joint pose constraint of the robot arm of the pre-grasping trajectory is transformed into adding the joint pose objective function The definition is as follows:

[0041]

[0042] Among them, ξ(t) represents the pre-grasp trajectory, q max ,q min are the maximum and minimum joint positions of the robot, respectively. The subscript j represents the parameter of the jth joint, i.e., the jth element of the vector represented by each symbol. m is the number of degrees of freedom of the robot, T is the total duration of the trajectory, and the function φ μ [·] is defined as:

[0043]

[0044] Where x represents the value in [] and μ is a definable parameter.

[0045] Furthermore, in step 3, the robot arm joint velocity constraint of the pre-grasping trajectory is transformed into adding the joint velocity objective function The definition is as follows:

[0046]

[0047] Among them, ξ (1) (t) represents the trajectory speed, v max represents the joint speed limit, the subscript j represents the parameter of the jth joint, that is, the jth element of the vector represented by each symbol, m is the number of degrees of freedom of the robot, T is the total duration of the trajectory, and the function φ μ [·] is defined as:

[0048]

[0049] Where x represents the value in [] and μ is a definable parameter.

[0050] Furthermore, in step 3, the robot joint acceleration constraint of the pre-grasping trajectory is transformed into adding the joint acceleration objective function The definition is as follows:

[0051]

[0052] Among them, ξ (2) (t) represents the trajectory acceleration, a max represents the joint acceleration limit, the subscript j represents the parameter of the jth joint, that is, the jth element of the vector represented by each symbol, m is the number of degrees of freedom of the robot, T is the total duration of the trajectory, and the function φ μ[·] is defined as:

[0053]

[0054] Where x represents the value in [] and μ is a definable parameter.

[0055] Furthermore, in step 3, the obstacle avoidance constraint is transformed into a simplified obstacle avoidance objective function: as follows:

[0056]

[0057] Among them, ξ(t) represents the pre-grasp trajectory, x z is the component of the position vector moving along the z-axis after positive kinematics solution. The z-axis is defined as the negative acceleration direction. obj is the upper surface height of the target to be grasped, T is the total duration of the trajectory, and the function ψ ε [·] are defined as follows:

[0058]

[0059] Here, x represents the value in [] and ε is another definable parameter.

[0060] Furthermore, in step 4, after the constraint transformation, it is transformed into the following unconstrained optimization problem:

[0061]

[0062] Among them, c represents the trajectory coefficient matrix, q represents the trajectory key point, T represents the trajectory duration vector, represents the initial objective function, * represents the symbol set * = {g, q, v, a, c}, which respectively correspond to the terminal pose objective function obtained based on the terminal constraint transformation, the joint pose objective function obtained based on the joint pose constraint transformation, the joint velocity objective function obtained based on the joint velocity constraint transformation, the joint acceleration objective function obtained based on the joint acceleration constraint transformation, and the obstacle avoidance objective function obtained based on the obstacle avoidance constraint transformation, ρ * represents the weight of each objective function, is the overall objective function.

[0063] Furthermore, in step 4, the gradient of the total objective function with respect to {q, T} is solved. The total objective function with respect to q i Gradient The calculation method is:

[0064]

[0065] where e (2i-1)s+1 is the identity matrix I 2MsThe ((2i-1)s+1)th column of is the total objective function, c represents the trajectory coefficient matrix, and the intermediate matrix Defined as:

[0066]

[0067] Among them, the transition matrix E i , Defined as:

[0068] E i =(β(T i ),β(T i ),...,β (2s-1 )(T i )) T

[0069] F i =(0,-β(0),...,-β (2s-1) (0)) T

[0070] Boundary transition matrix E M , Defined as:

[0071] F 0 =(β(0),...,β (s-1) (0)) T

[0072] E M =(β(T M ),...,β (s-1) (T M )) T

[0073] Where β is the basis of the trajectory curve, s is the order of the specified derivative value of the trajectory, and T M Represents the trajectory duration vector of the Mth pre-grabbed trajectory. Each item As the content of the i-th column of the matrix, the gradient of the total objective function with respect to the trajectory key point vector can be obtained

[0074] The total objective function is about T i Gradient The calculation method is:

[0075]

[0076] Where Tr{·} is the trace of the matrix. As the content of the i-th column of the vector, the gradient of the total objective function with respect to the trajectory duration vector can be obtained

[0077] calculate and After that, the unconstrained optimization problem can be solved using the L-BFGS solver.

[0078] A robot arm trajectory planning method device for grabbing a moving object on a conveyor belt comprises a memory and one or more processors, wherein the memory stores executable code, and when the one or more processors execute the executable code, they are used to implement the robot arm trajectory planning method for grabbing a moving object on a conveyor belt.

[0079] The advantages and beneficial effects of the present invention are:

[0080] The present invention proposes a robot arm trajectory planning method and device for grabbing moving objects on a conveyor belt, which can plan a trajectory of a robot arm grabbing a uniformly moving object on a conveyor belt within a limited time according to the real-time input position and speed of the object on the conveyor belt, and the trajectory has the local optimal property of balanced trajectory duration and smoothness, which enables the robot arm to quickly approach the dynamic object and accurately grab it, and meets the constraints of joint kinematics and obstacle avoidance. The present invention has a higher success rate and can perform fast and accurate grabbing at a higher conveyor belt movement speed. BRIEF DESCRIPTION OF THE DRAWINGS

[0081] Figure 1 It is a flow chart of the robot arm trajectory planning method in the present invention.

[0082] Figure 2 It is a schematic diagram of the robot arm in the present invention grabbing a moving object on a conveyor belt.

[0083] Figure 3a It is a speed curve of the robot arm grasping trajectory planned at a conveyor belt speed of 0.5 m / s in an embodiment of the present invention.

[0084] Figure 3b It is the acceleration curve of the robot arm grasping trajectory planned at a conveyor belt speed of 0.5 m / s in the embodiment of the present invention.

[0085] Figure 4 It is a structural schematic diagram of the robot arm trajectory planning device in the present invention. DETAILED DESCRIPTION

[0086] The specific implementation of the present invention is described in detail below in conjunction with the accompanying drawings. It should be understood that the specific implementation described here is only used to illustrate and explain the present invention, and is not used to limit the present invention.

[0087] like Figure 1 As shown, a robot arm trajectory planning method for grabbing a moving object on a conveyor belt includes the following steps:

[0088] Step 1: Define the problem of grasping dynamic objects on the conveyor belt and the robot arm trajectory representation method. The grasping of uniformly moving objects on the conveyor belt using a robot arm is defined as the planning of two trajectories: the pre-grasping trajectory and the grasping trajectory. Figure 2 As shown, the gripper of the end effector of the robot arm gradually moves to the lower right, forming a pre-grasping posture at the starting end of the grasping trajectory and a grasping posture at the end of the grasping trajectory.

[0089] First, the representation method and planning problem of the pre-grasping trajectory are defined. The pre-grasping trajectory is defined as the trajectory from the initial position of the robot to the pre-grasping position. The pre-grasping position is defined as the inverse kinematics of the pre-grasping position of the robot end effector moving along the negative direction of the approach vector for a distance h. For a serial robot with m degrees of freedom, the pre-grasping trajectory is represented by ξ(t), which is the time from time t to the robot joint space. The mapping of has a total trajectory duration T, is a piecewise function with the number of trajectory segments M, and its i-th trajectory ξ i (t) is defined as follows:

[0090]

[0091] where β(t)=(1,t,...,t 2s-1 ) T is the basis of the trajectory curve, is the coefficient matrix of the i-th trajectory, s is the order of the specified derivative value of the trajectory, t i-1 and t i is the minimum and maximum time limit of the trajectory, and the i-th trajectory has a duration of T i =t i -t i-1 For M segments of pre-captured trajectories, the duration of all trajectories constitutes the trajectory duration vector T = (T 1 ,...,T M ) T , the coefficient matrices of all segment trajectories constitute the trajectory coefficient matrix

[0092] The problem of pre-grasp trajectory planning is defined as solving the following optimization problem:

[0093]

[0094] stT>0

[0095] ξ(0)=q 0

[0096]

[0097] x(1) (ξ(T))=v c +v ⊥

[0098] ξ (2) (T) = 0

[0099]

[0100]

[0101]

[0102]

[0103] Where q=(q 1 ,...,q M ) is the key point of the trajectory, which contains the one-to-one corresponding robot arm posture in the trajectory time vector T. is the initial objective function, ρ is the time optimization weight, is the initial position of the robot arm, It is a positive kinematic mapping, which maps from joint space to Cartesian space is the conveyor belt speed, is the initial grasping posture, i.e. the grasping posture at the initial time t = 0, is the grasping posture approach vector, is the unitized grasping posture approach vector, is the collision-free joint space, q max ,q min are the maximum and minimum joint positions of the robot arm, v max ,a max are the joint speed limit and joint acceleration limit, ξ (1) (t),ξ (2) (t),ξ (3) (t) are trajectory velocity, trajectory acceleration and trajectory jerk, respectively.

[0104] Next, define the grasping trajectory and its generation method; the grasping trajectory is the trajectory from the pre-grasping posture to the grasping posture, and the generation method is:

[0105] Given the current position q of the robot arm j , the robot arm speed at the next moment is:

[0106]

[0107] Among them, J + is the pseudo-inverse of the Jacobian matrix of the robot. In this process, the robot gripper gradually closes until the robot reaches the grasping position and the grasping action phase ends.

[0108] Step 2: Convert the pre-grasp trajectory end constraint; convert the trajectory end pose constraint to add the end pose objective function The definition is as follows:

[0109]

[0110] Among them, ∈ is a small value, μ is a definable parameter, and the function φ μ [·] is defined as:

[0111]

[0112] q M is the pre-grasp pose of the current iteration, which is solved as follows:

[0113]

[0114] The terminal velocity constraint of the pre-grasp trajectory is transformed into:

[0115] ξ (1) (T) = J + (q M )(v c +v ⊥ ).

[0116] Step 3: Convert the kinematic constraints and collision constraints of the robot joints; convert the robot joint pose constraints of the pre-grasping trajectory into, and add the joint pose objective function The definition is as follows:

[0117]

[0118] Among them, the subscript j represents the parameter of the j-th joint, that is, the j-th element of the vector represented by each symbol.

[0119] The robot arm joint velocity constraint of the pre-grasp trajectory is transformed into adding the joint velocity objective function The definition is as follows:

[0120]

[0121] The joint acceleration constraint of the robot arm of the pre-grasp trajectory is transformed into adding the joint acceleration objective function The definition is as follows:

[0122]

[0123] The obstacle avoidance constraint is transformed into a simplified obstacle avoidance objective function. as follows:

[0124]

[0125] Among them, x z is the component of the position vector moving along the z-axis after positive kinematics solution. The z-axis is defined as the negative acceleration direction. obj is the upper surface height of the target to be grasped, function ψ ε [·] are defined as follows:

[0126]

[0127] Among them, ε is another definable parameter.

[0128] Step 4: Calculate the gradient of the transformed total objective function, use the optimization solver to solve it, and input the trajectory into the controller to control the robot arm to grab the object; after the constraint transformation in the previous steps, the above problem can be transformed into the following unconstrained optimization problem:

[0129]

[0130] where * represents the symbol set * = {g, q, v, a, c}, which corresponds one-to-one to the subscripts of the objective function defined above, ρ * represents the weight of each objective function, is the total number of targets.

[0131] Next, we solve the gradient of the total objective function with respect to {q,T}. The total objective function with respect to q i Gradient The calculation method is

[0132]

[0133] where e (2i-1)s+1 is the identity matrix I 2Ms The ((2i-1)s+1)th column of the intermediate matrix Defined as:

[0134]

[0135] Among them, the transition matrix E i , Defined as:

[0136] E i =(β(T i ),β(T i ),...,β (2s-1) (T i )) T

[0137] F i =(0,-β(0),...,-β (2s-1) (0))T

[0138] In particular, the boundary transition matrix E M , Defined as:

[0139] F 0 =(β(0),...,β (s-1) (0)) T

[0140] E M =(β(T M ),...,β (s-1) (T M )) T

[0141] Each item found As the content of the i-th column of the matrix, the gradient of the total objective function with respect to the trajectory key point vector can be obtained

[0142] The total objective function is about T i Gradient The calculation method is:

[0143]

[0144] Where Tr{·} is the trace of the matrix. As the content of the i-th column of the vector, the gradient of the total objective function with respect to the trajectory duration vector can be obtained

[0145] calculate and After that, the unconstrained optimization problem can be solved by L-BFGS solver. The solved trajectory is input into the controller to control the robot arm to grab the object.

[0146] An embodiment of the present invention:

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

[0148] 1) Define the problem of grasping dynamic objects on the conveyor belt and the robot arm trajectory representation method:

[0149] The planning of using a robotic arm to grab objects moving at a constant speed on a conveyor belt is divided into two trajectories: the pre-grabbing trajectory and the grasping trajectory.

[0150] The pre-grasp trajectory is defined as the trajectory from the initial position of the robot to the pre-grasp position. The pre-grasp position is defined as the inverse kinematics of the pre-grasp position of the robot end effector grasping posture moving back a distance h in the negative direction of the approach vector. For an m-degree-of-freedom serial robot, the robot m=7 used in the implementation, the pre-grasp trajectory is denoted as ξ(t), which is the time from time t to the robot joint space The mapping of has a total trajectory duration T, is a piecewise function with the number of trajectory segments M, and its i-th trajectory ξ i (t) is defined as follows:

[0151]

[0152] where β(t)=(1,t,...,t 2s-1 ) T is the basis of the trajectory curve, is the coefficient matrix of the i-th trajectory, s is the order of the specified derivative value of the trajectory, t i-1 and t i is the minimum and maximum time limit of the trajectory, and the i-th trajectory has a duration of T i =t i -t i-1 For M segments of pre-captured trajectories, the duration of all trajectories constitutes the trajectory duration vector T = (T 1 ,...,T M ) T , the coefficient matrices of all segment trajectories constitute the trajectory coefficient matrix In the implementation, s=3, M=3.

[0153] The problem of pre-grasp trajectory planning is defined as solving the following optimization problem:

[0154]

[0155] stT>0

[0156] ξ(0)=q 0

[0157]

[0158] x (1) (ξ(T))=v c +v ⊥

[0159] ξ (2) (T) = 0

[0160]

[0161]

[0162]

[0163]

[0164] Where q=(q 1 ,...,q M ) is the key point of the trajectory, which contains the one-to-one corresponding robot arm posture in the trajectory time vector T. is the initial objective function, ρ is the time optimization weight, is the initial position of the robot arm, It is a positive kinematic mapping, which maps from joint space to Cartesian space is the conveyor belt speed, is the initial grasping posture, i.e. the grasping posture at the initial time t = 0, is the grasping posture approach vector, is the unitized grasping posture approach vector, is the collision-free joint space. max ,q min are the maximum and minimum joint positions of the robot arm, v max ,a max are the joint speed limit and joint acceleration limit, ξ (1) (t),ξ (2) (t) are trajectory velocity and trajectory acceleration respectively.

[0165] In the implementation, we set ρ = 1500, q max =[360°,128.9°,360°,147.8°,360°,120.3°,360°] T ,q min =-q max ,q 0 =[0°,-1.6°,-175.1°,-92.1°,0°,-86.8°,94.5°] T , v c =0.5m / s.

[0166] Next, define the grasping trajectory and its generation method. The grasping trajectory is the trajectory from the pre-grasping posture to the grasping posture, and the generation method is:

[0167] Given the current position q of the robot arm j , the robot arm speed at the next moment is:

[0168]

[0169] Among them, J +is the pseudo-inverse of the Jacobian matrix of the robot. In this process, the robot gripper gradually closes until the robot reaches the grasping position, and the grasping action phase ends.

[0170] 2) Transform the pre-grasping trajectory end constraint. The trajectory end pose constraint is transformed into, adding the end pose objective function The definition is as follows:

[0171]

[0172] Among them, ∈ is a small value, μ is a definable parameter, and the function φ μ [·] is defined as:

[0173]

[0174] In the implementation, we set ∈ = 0.5 × 10 -3 , μ=1.

[0175] q M is the pre-grasp pose of the current iteration, which is solved as follows:

[0176]

[0177] The terminal velocity constraint of the pre-grasp trajectory is transformed into:

[0178] ξ (1) (T) = J + (q M )(v c (T)+v ⊥ )

[0179] 3) Convert the kinematic constraints and collision constraints of the robot joints. The robot joint pose constraints of the pre-grasping trajectory are converted to add the joint pose objective function The definition is as follows:

[0180]

[0181] Among them, the subscript j represents the parameter of the j-th joint, that is, the j-th element of the vector represented by each symbol.

[0182] The robot arm joint velocity constraint of the pre-grasp trajectory is transformed into adding the joint velocity objective function The definition is as follows:

[0183]

[0184] The joint acceleration constraint of the robot arm of the pre-grasp trajectory is transformed into adding the joint acceleration objective function The definition is as follows:

[0185]

[0186] The obstacle avoidance constraint is transformed into a simplified obstacle avoidance objective function. as follows:

[0187]

[0188] Among them, x z is the component of the position vector moving along the z-axis after positive kinematics solution. The z-axis is defined as the negative acceleration direction. obj is the upper surface height of the target to be grasped, function ψ ε [·] are defined as follows:

[0189]

[0190] Here, ε is another definable parameter. In the implementation, ε=0.02 is set.

[0191] The above four target functions need to be discretized in implementation:

[0192]

[0193] Where σ={q,v,a,c}, Represents the content of the integral symbol in the above objective function, and the weight coefficient vector is κ i is the discretization resolution, taking all κ i =20.

[0194] 4) Calculate the gradient of the transformed total objective function, use the optimization solver to solve it, and input the trajectory into the controller to control the robot arm to grab the object. After the constraint transformation in the previous steps, the above problem can be transformed into the following unconstrained optimization problem:

[0195]

[0196] where * represents the symbol set * = {g, q, v, a, c}, which corresponds one-to-one to the subscripts of the objective function defined above, ρ * represents the weight of each objective function, is the total objective function. In implementation, the weights of each objective function are set as:

[0197] ρ g =1×10 4 ,ρ q =1×10 4 ,ρ v =0.5×10 3 ,ρ a =1×10 4 ,ρc =1×10 3

[0198] Next, we solve the gradient of the total objective function with respect to {q,T}. The total objective function with respect to q i Gradient The calculation method is:

[0199]

[0200] where e (2i-1)s+1 is the identity matrix I 2Ms The ((2i-1)s+1)th column of the intermediate matrix Defined as:

[0201]

[0202] Among them, the transition matrix E i , Defined as:

[0203] E i =(β(T i ),β(T i ),...,β (2s-1 )(T i )) T

[0204] F i =(0,-β(0),...,-β (2s-1) (0)) T

[0205] In particular, the boundary transition matrix E M , Defined as:

[0206] F 0 =(β(0),...,β (s-1) (0)) T

[0207] E M =(β(T M ),...,β (s-1) (T M )) T

[0208] Each item found As the content of the i-th column of the matrix, the gradient of the total objective function with respect to the trajectory key point vector can be obtained

[0209] The total objective function is about T i Gradient The calculation method is:

[0210]

[0211] Where Tr{·} is the trace of the matrix. As the content of the i-th column of the vector, the gradient of the total objective function with respect to the trajectory duration vector can be obtained

[0212] calculate and After that, the unconstrained optimization problem can be solved by L-BFGS solver. The solved trajectory is input into the controller to control the robot arm to grab the object.

[0213] like Figure 3a , Figure 3b Shown are the speed and acceleration curves of the robot arm grasping trajectory planned under the set conveyor belt speed.

[0214] Corresponding to the aforementioned embodiment of a robot arm trajectory planning method for grabbing a moving object on a conveyor belt, the present invention also provides an embodiment of a robot arm trajectory planning device for grabbing a moving object on a conveyor belt.

[0215] See also Figure 4 A robot arm trajectory planning device for grabbing a moving object on a conveyor belt provided in an embodiment of the present invention includes a memory and one or more processors. The memory stores executable code. When the one or more processors execute the executable code, they are used to implement a robot arm trajectory planning method for grabbing a moving object on a conveyor belt in the above embodiment.

[0216] An embodiment of a robot arm trajectory planning device for grabbing a moving object on a conveyor belt of the present invention can be applied to any device with data processing capabilities, and the device with data processing capabilities can be a device or apparatus such as a computer. The device embodiment can be implemented through software, or through hardware or a combination of software and hardware. Taking software implementation as an example, as a device in a logical sense, it is formed by the processor of any device with data processing capabilities in which it is located reading the corresponding computer program instructions in the non-volatile memory into the internal memory for execution. From a hardware perspective, if Figure 4 As shown in the figure, it is a hardware structure diagram of a robot arm trajectory planning device for grabbing a moving object on a conveyor belt of the present invention, in which any device with data processing capability is located, except Figure 4 In addition to the processor, memory, network interface, and non-volatile memory shown, any device with data processing capabilities in which the apparatus in the embodiments is located may also include other hardware, generally based on the actual functions of the device with data processing capabilities, which will not be described in detail.

[0217] The implementation process of the functions and effects of each unit in the above-mentioned device is specifically described in the implementation process of the corresponding steps in the above-mentioned method, and will not be repeated here.

[0218] For the device embodiment, since it basically corresponds to the method embodiment, the relevant parts can refer to the partial description of the method embodiment. The device embodiment described above is only schematic, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the scheme of the present invention. Ordinary technicians in this field can understand and implement it without paying creative work.

[0219] An embodiment of the present invention further provides a computer-readable storage medium having a program stored thereon. When the program is executed by a processor, a robot arm trajectory planning method for grabbing a moving object on a conveyor belt in the above embodiment is implemented.

[0220] The computer-readable storage medium may be an internal storage unit of any device with data processing capability described in any of the aforementioned embodiments, such as a hard disk or a memory. The computer-readable storage medium may also be an external storage device of any device with data processing capability, such as a plug-in hard disk, a smart media card (SMC), an SD card, a flash card, etc. equipped on the device. Furthermore, the computer-readable storage medium may also include both an internal storage unit and an external storage device of any device with data processing capability. The computer-readable storage medium is used to store the computer program and other programs and data required by any device with data processing capability, and may also be used to temporarily store data that has been output or is to be output.

[0221] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some or all of the technical features thereof may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A robot arm trajectory planning method for grabbing a moving object on a conveyor belt, characterized in that: The following steps are involved: Step 1: Define the grasping of dynamic objects on the conveyor belt and the robot arm trajectory representation; define the grasping of uniformly moving objects on the conveyor belt by the robot arm as the pre-grasping trajectory and the grasping trajectory. The pre-grasping trajectory is the trajectory from the initial position of the robot arm to the pre-grasping position, and the grasping trajectory is the trajectory from the pre-grasping position to the grasping position. Step 2: Convert the end constraints of the pre-grasping trajectory; convert the end pose constraints of the pre-grasping trajectory into the end pose objective function, based on the constraints of the key points of the end trajectory in Cartesian space, and obtain the end velocity constraints of the pre-grasping trajectory after inverse kinematics calculation; Step 3: Convert the kinematic constraints and collision constraints of the robot joints; based on the pre-grasping trajectory of each joint and the maximum and minimum joint poses of the robot corresponding to the robot, convert the robot joint pose constraints of the pre-grasping trajectory; based on the trajectory speed of each joint and the maximum joint speed of the robot corresponding to the robot, convert the robot joint speed constraints of the pre-grasping trajectory; based on the trajectory acceleration of each joint and the maximum joint acceleration of the robot corresponding to the robot, convert the robot joint acceleration constraints of the pre-grasping trajectory; based on the pre-grasping trajectory and the upper surface height of the grasping target object, convert the obstacle avoidance constraint; Step 4: Based on the transformed constraints, construct the total objective function of the trajectory and calculate the gradient, obtain the gradient of the total objective function with respect to the trajectory key points and trajectory duration vector, use the optimization solver to solve the gradient, and input the obtained trajectory into the controller to control the robotic arm to grasp the target object.

2. A robot arm trajectory planning method for grabbing a moving object on a conveyor belt according to claim 1, characterized in that: The step 1 comprises the following specific steps: First, the representation method and planning problem of the pre-grasping trajectory are defined. The pre-grasping trajectory is defined as the trajectory from the initial position of the robot to the pre-grasping position. The pre-grasping position is defined as the inverse kinematics of the pre-grasping position of the end effector of the robot moving along the negative direction of the approach vector for a distance h. For a serial robot with m degrees of freedom, the pre-grasping trajectory is represented by ξ(t), which is the time from time t to the robot joint space. The mapping of has a total trajectory duration T, is a piecewise function with the number of trajectory segments M, and its i-th trajectory ξ i (t) is defined as follows: where β(t)=(1,t,...,t 2s-1 ) T is the basis of the trajectory curve, is the coefficient matrix of the i-th trajectory, s is the order of the specified derivative value of the trajectory, t i-1 and t i is the minimum and maximum time limit of the trajectory, and the i-th trajectory has a duration of T i =t i -t i-1 ; For M segments of pre-captured trajectories, all trajectory durations constitute the trajectory duration vector T = (T1,...,T M ) T , the coefficient matrices of all segment trajectories constitute the trajectory coefficient matrix The problem of pre-grasp trajectory planning is defined as solving the following optimization problem: stT>0 ξ(0)=q0 x (1) (ξ(T))=v c +v ⊥ x (2) (T)=0 Where q=(q1,...,q M ) is the key point of the trajectory, which contains the one-to-one corresponding robot arm posture in the trajectory time vector T. is the initial objective function, ρ is the time optimization weight, is the initial position of the robot arm, x: It is a positive kinematic mapping, which maps from joint space to Cartesian space is the conveyor belt speed, is the initial grasping posture, i.e. the grasping posture at the initial time t = 0, is the grasping posture approach vector, is the unitized grasping posture approach vector, is the collision-free joint space; q max ,q min are the maximum and minimum joint positions of the robot arm, v max ,a max are the joint speed limit and joint acceleration limit, ξ (1) (t),ξ (2) (t),ξ (3) (t) are trajectory velocity, trajectory acceleration and trajectory jerk, respectively; Then, define the grasping trajectory and its generation method; the grasping trajectory is the trajectory from the pre-grasping posture to the grasping posture, and the generation method is: Given the current position q of the robot arm j , the robot arm speed at the next moment is: Among them, J + is the pseudo-inverse of the Jacobian matrix of the robot arm. During this process, the robot arm gripper gradually closes until the robot arm reaches the grasping position, and the grasping action phase ends.

3. A robot arm trajectory planning method for grabbing a moving object on a conveyor belt according to claim 1, characterized in that: The step 2 is specifically divided into two stages: The trajectory end pose constraint is converted into adding the end pose objective function The definition is as follows: Among them, ξ(T) represents the pre-grasp trajectory function value when time t is the total trajectory length T, q M is the pre-grasp pose in the current iteration, ∈ is a small value, μ is a definable parameter, and the function φ μ [·] is defined as: Among them, q M Solved by: The terminal velocity constraint of the pre-grasp trajectory is transformed into: ξ (1) (T)=J + (q M )(v c +v ⊥ )。 4. A robot arm trajectory planning method for grabbing a moving object on a conveyor belt according to claim 1, characterized in that: In step 3, the joint pose constraint of the robot arm of the pre-grasping trajectory is transformed into adding the joint pose objective function The definition is as follows: Among them, ξ(t) represents the pre-grasp trajectory, q max ,q min are the maximum and minimum joint positions of the robot, respectively. The subscript j represents the parameter of the jth joint, i.e., the jth element of the vector represented by each symbol. m is the number of degrees of freedom of the robot, T is the total duration of the trajectory, and the function φ μ [·] is defined as: Where x represents the value in [] and μ is a definable parameter.

5. A robot arm trajectory planning method for grabbing a moving object on a conveyor belt according to claim 1, characterized in that: In step 3, the robot arm joint velocity constraint of the pre-grasping trajectory is transformed into adding the joint velocity objective function The definition is as follows: Among them, ξ (1) (t) represents the trajectory speed, v max represents the joint speed limit, the subscript j represents the parameter of the jth joint, that is, the jth element of the vector represented by each symbol, m is the number of degrees of freedom of the robot, T is the total duration of the trajectory, and the function φ μ [·] is defined as: Where x represents the value in [] and μ is a definable parameter.

6. A robot arm trajectory planning method for grabbing a moving object on a conveyor belt according to claim 1, characterized in that: In step 3, the robot joint acceleration constraint of the pre-grasping trajectory is transformed into adding the joint acceleration objective function The definition is as follows: Among them, ξ (2) (t) represents the trajectory acceleration, a max represents the joint acceleration limit, the subscript j represents the parameter of the jth joint, that is, the jth element of the vector represented by each symbol, m is the number of degrees of freedom of the robot, T is the total duration of the trajectory, and the function φ μ [·] is defined as: Where x represents the value in [] and μ is a definable parameter.

7. A robot arm trajectory planning method for grabbing a moving object on a conveyor belt according to claim 1, characterized in that: In step 3, the obstacle avoidance constraint is transformed into a simplified obstacle avoidance objective function: as follows: Among them, ξ(t) represents the pre-grasp trajectory, x z is the component of the position vector moving along the z-axis after positive kinematics solution. The z-axis is defined as the negative acceleration direction. obj is the upper surface height of the target to be grasped, T is the total duration of the trajectory, and the function ψ ε [·] are defined as follows: Here, x represents the value in [] and ε is another definable parameter.

8. A robot arm trajectory planning method for grabbing a moving object on a conveyor belt according to claim 1, characterized in that: In step 4, after the constraint transformation, it is transformed into the following unconstrained optimization problem: Among them, c represents the trajectory coefficient matrix, q represents the trajectory key point, T represents the trajectory duration vector, represents the initial objective function, * represents the symbol set * = {g, q, v, a, c}, which respectively correspond to the terminal pose objective function obtained based on the terminal constraint transformation, the joint pose objective function obtained based on the joint pose constraint transformation, the joint velocity objective function obtained based on the joint velocity constraint transformation, the joint acceleration objective function obtained based on the joint acceleration constraint transformation, and the obstacle avoidance objective function obtained based on the obstacle avoidance constraint transformation, ρ * represents the weight of each objective function, is the overall objective function.

9. A robot arm trajectory planning method for grabbing a moving object on a conveyor belt according to claim 1, characterized in that: In step 4, the gradient of the total objective function with respect to {q, T} is solved. The total objective function with respect to q i Gradient The calculation method is: where e (2i-1)s+1 is the identity matrix I 2Ms The ((2i-1)s+1)th column of is the total objective function, c represents the trajectory coefficient matrix, and the intermediate matrix Defined as: The transition matrix Defined as: E i =(β(T i ),β(T i ),...,b (2s-1) (T i )) T F i ＝(0,-β(0),...,-β (2s-1) (0)) T Boundary transition matrix E M , Defined as: F0＝(β(0),...,β (s-1) (0)) T E M =(β(T M ),...,b (s-1) (T M )) T Where β is the basis of the trajectory curve, s is the order of the specified derivative value of the trajectory, and T M Represents the trajectory duration vector of the Mth pre-grabbed trajectory. Each item As the content of the i-th column of the matrix, the gradient of the total objective function with respect to the trajectory key point vector can be obtained The total objective function is about T i Gradient The calculation method is: Where Tr{·} is the trace of the matrix. As the content of the i-th column of the vector, the gradient of the total objective function with respect to the trajectory duration vector can be obtained 10. A robot arm trajectory planning method and device for grabbing a moving object on a conveyor belt, characterized in that: It comprises a memory and one or more processors, wherein the memory stores executable codes, and when the one or more processors execute the executable codes, they are used to implement a robot arm trajectory planning method for grabbing a moving object on a conveyor belt as described in any one of claims 1-9.

Images