Coordinated motion planning method for double-arm robot under multiple constraints

Abstract

The invention provides a double-arm robot coordinated motion planning method under multiple constraints, which belongs to the field of robots, and comprises the following steps of: establishing a task constraint mathematical model according to a double-arm robot coordinated operation task demand and a target by acquiring target information and a current state of a robot; a quadratic penalty function method is introduced to convert end effector position closed-chain equality constraint relations in two-arm coordinated operation tasks in different scenes into a target function, and a multi-constraint optimization problem of two-arm robot coordinated motion planning is constructed; solving by adopting a model prediction path integral control method based on an information theory to obtain a double-arm coordinated motion optimal track meeting a multi-constraint task requirement; and finally, applying the optimal trajectory of the two-arm coordinated motion to the robot simulation operation task. According to the method, the working efficiency and the stability of the double-arm robot during completion of a smart coordinated motion planning task can be effectively improved, and a new theoretical and method basis is provided for practical application of the double-arm robot in a complex scene.

Description

Technical Field

[0001] The present invention belongs to the technical field of robotics, and particularly relates to a coordinated motion planning method for a dual-arm robot under multiple constraints. Background Art

[0002] With the gradual wide application of dual-arm robots in dexterous operation tasks, it has become a necessary requirement to improve the coordinated motion planning level of dual-arm robots for them to complete dexterous operation tasks under multiple constraints. However, traditional planning methods, such as master-slave control and impedance control, are difficult to effectively handle the motion coupling problem under multiple constraints (especially closed-chain constraints) when the task complexity increases. In recent years, optimization algorithms based on kinematic and dynamic models have made remarkable progress in this field. However, current related methods for coordinated and fast motion planning of dual arms under closed-chain constraints face key problems such as difficulty in balancing optimization efficiency and trajectory optimality, and thus cannot be widely applied in actual dual-arm coordinated motion planning tasks. Summary of the Invention

[0003] To solve the drawbacks and deficiencies of the prior art, a coordinated motion planning method for a dual-arm robot under multiple constraints is provided, so as to solve the problem that it is difficult for current dual-arm robot coordinated motion planning methods to balance optimization efficiency and trajectory optimality.

[0004] A coordinated motion planning method for a dual-arm robot under multiple constraints provided for achieving the purpose of the present invention is as follows:

[0005] S1. Obtain the target information and current state of the dual-arm robot;

[0006] S2. According to the requirements and objectives of the coordinated operation task of the dual-arm robot, establish a mathematical model of task constraints, introduce the quadratic penalty function method to transform the closed-chain equality constraints of the end positions in the coordinated operation tasks of the dual-arm robot in different scenarios into the objective function, and construct a multi-constraint optimization problem for the coordinated motion planning of the dual-arm robot;

[0007] S3. For the multi-constraint optimization problem, use the model predictive path integral control method based on information theory to solve it, and obtain the optimal trajectory of the dual-arm coordinated motion that meets the requirements of the multi-constraint task;

[0008] S4. Apply the optimal trajectory of the dual-arm coordinated motion to the simulation operation task of the dual-arm robot.

[0009] As a further improvement of the above solution, the target information in step S1 is the pose determined by the operation object; the current state in step S1 includes the joint angles of the dual-arm robot, control actions, and the Cartesian space pose of the end effectors of the dual arms.

[0010] As a further improvement of the above solution, the coordinated operation task of the dual-arm robot in step S2 mainly includes the coordinated operation task in the Cartesian space. The constraint of the coordinated operation task in the Cartesian space mainly aims at the dual-arm closed-chain constraint, and the dual-arm closed-chain constraint means that the relative pose between the end effectors of the two arms remains unchanged during the movement after grasping an object.

[0011] As a further improvement of the above solution, the closed-chain equality constraint in step S2 is represented by h(t, x t ) = 0, and the specific form is as follows:

[0012]

[0013] Among them, ||q r -q l || 2 = 0 means that the Euclidean distance between the postures of the end effector of the right arm and the end effector of the left arm of the robot is zero; represents that the Euclidean distance between the position of the end effector of the right arm of the robot to the right target point and the position of the end effector of the left arm to the left target point is zero.

[0014] As a further improvement of the above solution, in step S2, the quadratic penalty function method is introduced to change the closed-chain equality constraint into a soft constraint, forming an augmented cost function, and its expression is as follows:

[0015]

[0016] Among them, ||q r -q l || 2 represents the Euclidean distance between the postures of the end effector of the right arm and the end effector of the left arm of the robot; represents the Euclidean distance between the position of the end effector of the right arm of the robot to the right target point and the position of the end effector of the left arm to the left target point; w 6 、w 7 are weight coefficients used to balance the influence of the quadratic penalty term on the total loss.

[0017] As a further improvement of the above solution, the expression of s(x t ) in the augmented cost function is as follows:

[0018]

[0019] Among them, w 1 、w 2 、w 3 、w 4 are weight coefficients used to balance the influence of each penalty term on the total loss;

[0020] represent each penalty term, specifically represents the Euclidean distance between the end effector of the robot's right arm and the position of the right target point; represents the Euclidean distance between the end effector of the robot's left arm and the position of the left target point; represents the Euclidean distance between the end effector of the robot's right arm and the pose of the right target point, and the pose is represented by a quaternion; represents the Euclidean distance between the end effector of the robot's left arm and the pose of the left target point;

[0021] k energy represents the energy term, which is usually used to penalize too fast speed to ensure the safety of the dual-arm movement, and is usually expressed in the form of v 2 , where v represents the sampled input;

[0022] e r and e l represent the reward terms, which are used to verify that the model predictive path integral control algorithm can handle discontinuous cost functions. The expression is as follows:

[0023]

[0024] As a further improvement of the above solution, based on the closed-chain equality constraint conditions, comprehensively considering the input energy of the robot's dual arms, the reward terms, and the pose error between the end effector and the operating object, a coordinated motion planning optimization model for the dual-arm robot under multiple constraints is constructed as follows:

[0025]

[0026] s.t. x t＝0 = x 0 ,

[0027] x t+1 = F(x t , v t ),

[0028] h(t, x t ) = 0;

[0029] where V = {v 0 , v 1 ,..., v T-1} is the input sequence; t ∈ {0, 1,..., T - 1} is the length of the prediction horizon;

[0030] s(x t ) is the cost at the instant of state x t ; To penalize the sensitivity of the control input to noise, where λ is the regularization coefficient used to balance control efficiency and noise sensitivity, is the transpose of the control input, ∑ -1 is the inverse matrix of the noise covariance matrix, is the noise or random perturbation, usually assumed to be Gaussian noise

[0031] x t＝0 = x 0 is the initial state, x t+1 = F(x t , v t ) is the dynamic equation of the system, solved by the Pinocchio library;

[0032] h(t, x t ) = 0 is the closed-chain equality constraint, which will be appropriately added to the cost s(x t ).

[0033] As a further improvement to the above scheme, the multi-constraint optimization problem in step S2 is formulated as minimizing the expected value of the input sequence state cost in the sampling distribution.

[0034] As a further improvement to the above scheme, the information-theoretic model predictive path integral control method in step S3 randomly samples the predicted trajectory through a Gaussian noise model, and analyzes the error between the predicted trajectory samples and the desired trajectory using the KL divergence theory in information theory; then uses the importance sampling method to calculate different weights for the predicted trajectory samples and obtains the optimized trajectory for the dual-arm coordinated motion that satisfies multiple constraints through the sample summation method.

[0035] As a further improvement to the above scheme, in the information-theoretic model predictive path integral control method, the initial state x 0 of the robot and the sampled input v t are used to generate the importance sampling trajectory H, and then multiple constraint conditions such as the task constraint, energy constraint, and end position closed-chain constraint of the dual-arm coordinated motion are added to the calculation of the trajectory cost C(V) based on H. Finally, the robot executes the optimized update result to obtain the optimal solution of the multi-constraint optimization problem and transmits its control input sequence U and the initial state x 0 to the next optimization update loop, specifically as follows:

[0036]

[0037] Where: K represents the number of predicted trajectory samples; w(ε k ) is the weight of each trajectory prediction sample obtained by the importance sampling method; the control input sequence U = {u 0 , u1 ,...,u T-1};

[0038] Sampling input Among them, represents the Gaussian white noise model.

[0039] The beneficial effects of the present invention are as follows:

[0040] Compared with the prior art, a method for coordinated motion planning of a dual-arm robot under multiple constraints provided by the present invention requires constructing various forms of constraints for the coordinated motion of the dual arms, including task constraints, end-position closed-chain constraints, energy constraints, etc.; introducing the quadratic penalty function method to transform the end-position closed-chain equality constraint relationship in the dual-arm coordinated operation task under different scenarios into the objective function, and combining various forms of constraints to construct an optimization problem for the dual-arm coordinated operation task; a stochastic optimal dual-arm coordinated motion planning strategy based on the model predictive path integral control method can handle continuous or discontinuous cost functions to solve the high-dimensional motion coordination optimization problem of the dual arms.

[0041] In summary, a method for coordinated motion planning of a dual-arm robot under multiple constraints provided by the present invention provides a new theoretical and method basis for the practical application of the robot's dual arms in complex scenarios. Brief Description of the Drawings

[0042] Figure 1 is a schematic flow chart of the present invention;

[0043] Figure 2 is a schematic flow chart of each step in the present invention. Detailed Description of the Preferred Embodiments

[0044] The following further details the specific embodiments of the present invention with reference to the accompanying drawings:

[0045] Embodiment 1

[0046] According to Figure 1 - Figure 2 shown, the present invention provides a method for coordinated motion planning of a dual-arm robot under multiple constraints, including the following steps:

[0047] S1. Obtain the target information and current state of the dual-arm robot.

[0048] Among them, the target information is the pose determined by the operation object; the current state of the robot includes the joint angles of the dual-arm robot, the control actions, and the Cartesian space pose of the end effectors of the dual arms.

[0049] S2. According to the requirements and goals of the coordinated operation task of the dual-arm robot, establish a mathematical model of task constraints, introduce the quadratic penalty function method to transform the closed-loop equality constraints of the end positions in the coordinated operation task of the dual-arm robot under different scenarios into the objective function, and construct a multi-constraint optimization problem for the coordinated motion planning of the dual-arm robot.

[0050] Among them, the coordinated operation task of the dual-arm robot mainly includes the coordinated operation task of the two arms in the Cartesian space, such as Figure 2 as shown in Fig. a. The constraints of the coordinated operation task of the two arms in the Cartesian space mainly target the closed-loop constraints of the two arms. The closed-loop constraints mean that the relative pose between the end effectors of the two arms remains unchanged during the movement after grasping an object.

[0051] The closed-loop equality constraint is represented by h(t, x t ) = 0, and the specific form is as follows:

[0052]

[0053] Among them, ||q r -q l || 2 = 0 means that the Euclidean distance between the postures of the end effector of the right arm and the end effector of the left arm of the robot is zero; represents that the Euclidean distance between the position of the end effector of the right arm of the robot to the right target point and the position of the end effector of the left arm to the left target point is zero.

[0054] Introduce the quadratic penalty function method to change the closed-loop equality constraint into a soft constraint, and form an augmented cost function, whose expression is as follows:

[0055]

[0056] Among them, ||q r -q l || 2 represents the Euclidean distance between the postures of the end effector of the right arm and the end effector of the left arm of the robot; represents the Euclidean distance between the position of the end effector of the right arm of the robot to the right target point and the position of the end effector of the left arm to the left target point. w 6 , w 7 are weight coefficients, which are used to balance the influence of the quadratic penalty term on the total loss.

[0057] The expression of s(x t ) in the augmented cost function is as follows:

[0058]

[0059] Among them, w 1 , w 2 , w3 , w 4 is a weight coefficient, which is used to balance the influence of each penalty term on the total loss respectively;

[0060] represents each penalty term. Specifically, represents the Euclidean distance between the end effector of the robot's right arm and the position of the right target point; represents the Euclidean distance between the end effector of the robot's left arm and the position of the left target point; represents the Euclidean distance between the end effector of the robot's right arm and the pose of the right target point, and the pose is represented by a quaternion; represents the Euclidean distance between the end effector of the robot's left arm and the pose of the left target point;

[0061] k energy represents the energy term, which is usually used to penalize too fast speed to ensure the safety of the two-arm movement. It is usually represented in the form of v 2 , where v represents the sampled input;

[0062] e r and e l represent the reward terms, which are used to verify that the model predictive path integral control algorithm can handle discontinuous cost functions. The expression is as follows:

[0063]

[0064] Based on the closed-chain equality constraint conditions, considering the input energy of the robot's two arms, the reward terms, and the pose error between the end effector and the object to be operated, a coordinated motion planning optimization model for the two-arm robot under multiple constraints is constructed, as shown in Figure 2 .b. Specifically as follows:

[0065]

[0066] s.t. x t＝0 = x 0 ,

[0067] x t+1 = F(x t , v t ),

[0068] h(t, x t ) = 0;

[0069] where V = {v 0 , v 1 ,..., v T-1} is the input sequence; t ∈ {0, 1,..., T - 1} is the length of the prediction horizon;

[0070] s(x t) is the state x t The immediate cost; used to penalize the sensitivity of the control input to noise, where λ is the regularization coefficient for balancing control efficiency and noise sensitivity, is the transpose of the control input, ∑ -1 is the inverse matrix of the noise covariance matrix, is the noise or random perturbation, usually assumed to be Gaussian noise

[0071] x t＝0 = x 0 is the initial state, x t+1 = F(x t , v t ) is the dynamic equation of the system, solved by the Pinocchio library;

[0072] h(t, x t ) = 0 is the closed-chain equality constraint, which will be appropriately added to the cost s(x t ).

[0073] The multi-constraint optimization problem is formulated as minimizing the expected value of the state cost of the input sequence in the sampling distribution.

[0074] S3. For the multi-constraint optimization problem, an information-theoretic model predictive path integral control method is used to solve it, and the optimal trajectory of the dual-arm coordinated motion that meets the multi-constraint task requirements is obtained, as shown in Figure 2 .c

[0075] Among them, the information-theoretic model predictive path integral control method randomly samples the predicted trajectory through a Gaussian noise model, and analyzes the error between the predicted trajectory samples and the expected trajectory using the KL divergence theory in information theory; then the importance sampling method is used to calculate different weights for the predicted trajectory samples, and the optimal trajectory of the dual-arm coordinated motion that meets multiple constraints is obtained by summing the samples.

[0076] In the information-theoretic model predictive path integral control method, the core model content is:

[0077]

[0078] Among them, S(V k ) represents the cost of the sampling trajectory, where is the terminal cost, representing the penalty for the final state x T ; s(x t ) represents the immediate cost of the state x t ; in the dual-arm coordinated motion, the terminal cost is the cost of the target deviation, which is already included in s(x t ), so Specifically represents the cumulative cost of the k-th trajectory sample from t = 0 to T-1;

[0079] Used to penalize the sensitivity of the control input to noise, where λ is the regularization parameter, used to balance control efficiency and noise sensitivity, is the transpose of the control input, ∑ -1 is the inverse matrix of the noise covariance matrix, is the noise or random perturbation, usually assumed to be Gaussian noise

[0080] C(V k ) is the trajectory error of the k-th trajectory sample obtained through the KL divergence theory; β is the minimum value of the trajectory errors {C(V 0 ), C(V 1 ),... C(V K-1 )}; η is the normalization constant; w(ε k ) is the weight of each trajectory prediction sample obtained through the importance sampling method.

[0081] In the model predictive path integral control method based on information theory, the initial state x of the robot is utilized 0 and the sampled input v t to generate the importance sampling trajectory H. Then, multiple constraint conditions such as the task constraint, energy constraint, and end position closed-chain constraint of the dual-arm coordinated motion are added to the calculation of the trajectory cost C(V) based on H. Finally, the robot executes the optimized update result to obtain the optimal solution of the multi-constraint optimization problem and transmits its control input sequence U and the initial state x 0 to the next optimization update loop, specifically as follows:

[0082]

[0083] where: K represents the number of predicted trajectory samples; w(ε k ) is the weight of each trajectory prediction sample obtained through the importance sampling method; the control input sequence U = {u 0 , u 1 ,.... u T-1};

[0084] Sampled input wherein represents the Gaussian white noise model.

[0085] S4. Apply the optimal trajectory of the dual-arm coordinated motion to the simulation operation task of the dual-arm robot.

[0086] The present invention obtains the target information of the robot and the current state of the robot; according to the requirements and objectives of the coordinated operation task of the dual-arm robot, a mathematical model of task constraints is established, and a multi-constraint optimization problem for the coordinated motion planning of the dual-arm robot is constructed; for the multi-constraint optimization problem, a model predictive path integral control method based on information theory is used to solve it, and the optimal trajectory of the dual-arm coordinated motion that meets the multi-constraint task requirements is obtained; the optimal trajectory of the dual-arm coordinated motion is applied to the simulation operation task of the robot. The method for dual-arm coordinated motion planning under multiple constraints of the present invention improves the working efficiency and stability of the dual-arm robot when completing the dexterous coordinated motion planning task, and provides new theories and methods for the practical application of the robot's dual arms in complex scenarios.

[0087] The above embodiments are not limited to the technical solutions of the embodiments themselves, and the embodiments can be combined with each other to form new embodiments. The above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit them. Any modification or equivalent replacement that does not depart from the spirit and scope of the present invention shall be covered by the technical solutions of the present invention.

Claims

1. A method for coordinated motion planning of a dual-arm robot under multiple constraints, characterized in that: The specific steps are as follows: S1, obtain the target information and current state of the dual-arm robot; S2, according to the requirements and goals of the dual-arm robot coordinated operation task, a mathematical model of task constraints is established, and the quadratic penalty function method is introduced to transform the end position closed-chain equality constraints in the dual-arm robot coordinated operation task under different scenarios into the objective function, and the multi-constraint optimization problem of the dual-arm robot coordinated motion planning is constructed; S3, for multi-constraint optimization problems, a model prediction path integral control method based on information theory is used to solve them and obtain the optimal trajectory of dual-arm coordinated motion that meets the requirements of multi-constraint tasks; S4, applies the optimal trajectory of dual-arm coordinated motion to the simulation manipulation task of the dual-arm robot.

2. A method for coordinated motion planning of a dual-arm robot under multiple constraints according to claim 1, characterized in that: The target information in step S1 is the determined posture of the operation object; the current state in step S1 includes the joint angles of the dual-arm robot, the control actions and the Cartesian space posture of the dual-arm end effector.

3. The method for coordinated motion planning of a dual-arm robot under multiple constraints according to claim 1, characterized in that: The dual-arm robot coordinated operation task in step S2 mainly includes the dual-arm coordinated operation task in Cartesian space. The dual-arm coordinated operation task constraints in Cartesian space are mainly for the dual-arm closed-chain constraints. The dual-arm closed-chain constraints indicate that the relative posture between the dual-arm end effectors remains unchanged during the movement after grasping the object.

4. The method for coordinated motion planning of a dual-arm robot under multiple constraints according to claim 3 is characterized in that: The closed chain equality constraint in step S2 is given by h(t, x t )=0, the specific form is as follows: Among them, ||q r -q l ||2=0 means that the Euclidean distance between the robot's right arm end effector and the left arm end effector posture is zero. The Euclidean distance representing the difference between the position of the robot's right arm end effector to the right target point and the position of the left arm end effector to the left target point is zero.

5. The method for coordinated motion planning of a dual-arm robot under multiple constraints according to claim 4, characterized in that: In step S2, the quadratic penalty function method is introduced to transform the closed-chain equality constraint into a soft constraint, thereby forming an augmented cost function, which is expressed as follows: Among them, ||q r -q l ||2 represents the Euclidean distance between the postures of the robot's right arm end effector and the left arm end effector; It represents the Euclidean distance between the position of the robot's right arm end effector to the right target point and the position of the left arm end effector to the left target point. w6 and w7 are weight coefficients used to balance the impact of the quadratic penalty term on the total loss.

6. The method for coordinated motion planning of a dual-arm robot under multiple constraints according to claim 5, characterized in that: The augmented cost function s(x t ) is as follows: Among them, w1, w2, w3, and w4 are weight coefficients, which are used to balance the impact of each penalty item on the total loss; Represents each penalty item, specifically It represents the Euclidean distance between the end effector of the robot's right arm and the target point on the right side; It represents the Euclidean distance between the robot's left arm end effector and the left target point; represents the Euclidean distance between the robot's right arm end effector and the right target point posture, and the posture is represented by quaternion; It represents the Euclidean distance between the left arm end effector of the robot and the left target point posture; k energy Represents the energy term, which is usually used to penalize excessive speed to ensure the safety of double-arm movement, usually expressed as v 2 It is expressed in the form of, where v represents the sample input; e r and e l Represents the reward term, which is used to verify that the model-predicted path integral control algorithm can handle discontinuous cost functions. The expression is as follows:

7. The method for coordinated motion planning of a dual-arm robot under multiple constraints according to claim 6, characterized in that: Based on the closed-chain equation constraint conditions, the input energy of the robot's dual arms, the reward item, and the posture error between the end effector and the operation object are comprehensively considered to construct a coordinated motion planning optimization model of the dual-arm robot under multiple constraints, as follows: s.t.x t＝0 =x0, x t+1 =F(x t ,v t ), h(t,x t )=0; Where V = {v0, v1, ..., v T-1 } is the input sequence; t∈{0, 1, ...T-1} is the length of the prediction time domain; s(x t ) is the state x t Immediate cost; It is used to penalize the sensitivity of the control input to noise, where λ is the regularization coefficient, which is used to balance the control efficiency and noise sensitivity. is the transpose of the control input, ∑ -1 is the inverse of the noise covariance matrix, is noise or random disturbance, usually assumed to be Gaussian noise x t＝0 =x0 is the initial state, x t+1 =F(X t , v t ) is the dynamic equation of the system, which is solved by Pinocchio Library; h(t,x t )=0 is a closed-chain equality constraint, and the cost s(x t )middle.

8. The method for coordinated motion planning of a dual-arm robot under multiple constraints according to claim 7, characterized in that: The multi-constraint optimization problem in step S2 is expressed as minimizing the expected value of the input sequence state cost in the sampling distribution.

9. The method for coordinated motion planning of a dual-arm robot under multiple constraints according to claim 1, characterized in that: The model prediction path integral control method based on information theory in step S3 is to randomly sample the predicted trajectory through a Gaussian noise model, and use the KL divergence theory in information theory to analyze the error between the predicted trajectory sample and the expected trajectory; then use the importance sampling method to assign different weights to the predicted trajectory samples for calculation, and obtain the optimal trajectory of the coordinated movement of the two arms that meets multiple constraints through sample summation.

10. The method for coordinated motion planning of a dual-arm robot under multiple constraints according to claim 9, characterized in that: The model prediction path integral control method based on information theory uses the robot's initial state x0 and sampled input v t Generate the importance sampling trajectory H, then add multiple constraints such as the task constraint of the coordinated motion of the two arms, the energy constraint, and the end position closed chain constraint to the calculation of the trajectory cost C(V) based on H. Finally, the robot executes the optimization update result to obtain the optimal solution of the multi-constraint optimization problem. And pass its control input sequence U and initial state x0 to the next optimization update cycle, as follows: Where: K represents the number of predicted trajectory samples; w(ε k ) is the weight of each trajectory prediction sample obtained by the importance sampling method; the control input sequence U = {u0,u1,...u T-1 }; Sampling Input Among them represents the Gaussian white noise model.

Images