A method for planning coordinated motion of dual-arm robots under multiple constraints

Abstract

The application provides a double-arm robot coordinated motion planning method under multiple constraints, and belongs to the field of robots. The method obtains robot target information and a current state, establishes a mathematical model of a task constraint according to a double-arm robot coordinated operation task demand and a target, introduces a quadratic penalty function method to convert end effector position closed chain equation constraint relationships in double-arm coordinated operation tasks in different scenes into a target function, and constructs a multiple constraint optimization problem of double-arm robot coordinated motion planning. Then, a model prediction path integral control method based on information theory is used for solving, so that an optimal trajectory of double-arm coordinated motion meeting multiple constraint task demands is obtained. Finally, the optimal trajectory of double-arm coordinated motion is applied to a simulation operation task of the robot. The method can effectively improve the work efficiency and stability of the double-arm robot when completing a dexterous coordinated motion planning task, and provides a new theoretical and method basis for practical application of the double-arm robot in complex scenes.

Description

Technical Field

[0001] This invention belongs to the field of robotics technology, and in particular relates to a method for coordinated motion planning of a dual-arm robot under multiple constraints. Background Technology

[0002] As dual-arm robots are increasingly used in dexterity tasks, enabling them to perform dexterity tasks under multiple constraints has become a crucial requirement for improving the coordinated motion planning capabilities of dual-arm robots. However, traditional planning methods, such as master-slave control and impedance control, struggle to effectively handle motion coupling problems under multiple constraints (especially closed-chain constraints) as task complexity increases. In recent years, optimization algorithms based on kinematic and dynamic models have made significant progress in this field. However, current methods for coordinated and rapid motion planning of dual-arm robots under closed-chain constraints face key challenges in balancing optimization efficiency and trajectory optimality, hindering their widespread application in practical dual-arm coordinated motion planning tasks. Summary of the Invention

[0003] To address the shortcomings and deficiencies of existing technologies, a method for coordinated motion planning of dual-arm robots under multiple constraints is provided, which can solve the problem that current methods for coordinated motion planning of dual-arm robots cannot simultaneously achieve optimization efficiency and trajectory optimality.

[0004] To achieve the objectives of this invention, a coordinated motion planning method for a dual-arm robot under multiple constraints is provided, the specific steps of which are as follows:

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

[0006] S2. Based on the requirements and objectives of the coordinated operation task of the dual-arm robot, a mathematical model of task constraints is established. The quadratic penalty function method is introduced to transform the end position closed chain equation constraints in the coordinated operation task of the dual-arm robot under different scenarios into the objective function, and a multi-constraint optimization problem of coordinated motion planning of dual-arm robot is constructed.

[0007] S3. For multi-constraint optimization problems, the model prediction path integral control method based on information theory is used to solve the problem and obtain the optimal trajectory of the coordinated movement of the two arms that meets the requirements of multi-constraint tasks.

[0008] S4 applies the optimal trajectory of coordinated bi-arm movement to the simulation operation task of the bi-arm robot.

[0009] As a further improvement to the above scheme, 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, the control actions, and the Cartesian space pose of the dual-arm end effectors.

[0010] As a further improvement to the above scheme, the dual-arm robot coordinated operation task in step S2 mainly includes the dual-arm coordinated operation task in Cartesian space. The constraints of the dual-arm coordinated operation task in Cartesian space are mainly for the dual-arm closed chain constraint, which means that the relative pose between the end effectors of the two arms remains unchanged during the motion after grasping the object.

[0011] As a further improvement to the above scheme, the closed-chain equality constraint in step S2 is determined by h(t,x) t ) = 0 indicates that the specific form is as follows:

[0012]

[0013] Among them, ||q r -q l ||2=0 indicates that the Euclidean distance between the attitudes of the robot's right arm end effector and the left arm end effector is zero; The Euclidean distance between the position of the robot's right arm end effector to the target point on the right and the position of the left arm end effector to the target point on the left is zero.

[0014] As a further improvement to the above scheme, step S2 introduces a quadratic penalty function method to transform the closed-chain equality constraint into a soft constraint, forming an augmented cost function, the expression of which is as follows:

[0015]

[0016] Among them, ||q r -q l ||2 represents the Euclidean distance between the attitudes of the robot's right arm end effector and the left arm end effector; represents the Euclidean distance between the position of the robot's right arm end effector to the target point on the right and the position of the left arm end effector to the target point on the left; w6 and w7 are weighting coefficients used to balance the impact of the quadratic penalty term on the total loss.

[0017] As a further improvement to the above scheme, the augmented cost function s(x) t The expression for ) is as follows:

[0018]

[0019] Among them, w1, w2, w3, and w4 are weighting coefficients, which are used to balance the impact of each penalty term on the total loss.

[0020] This indicates the various penalty items, specifically... This represents the Euclidean distance between the robot's right arm end effector and the target point on the right side; This represents the Euclidean distance between the robot's left arm end effector and the target point on the left side; The Euclidean distance between the robot's right arm end effector and the target point on the right side is represented by a quaternion. This represents the Euclidean distance between the robot's left arm end effector and the target point on the left side;

[0021] k energy This represents an energy term, typically used to penalize excessive speed to ensure safe arm movements, usually expressed as v. 2 The form is represented as , where v represents the sampled input;

[0022] e r and e l This represents the reward term, used to verify that the model's path integral control algorithm can handle discontinuous cost functions. The expression is as follows:

[0023]

[0024] As a further improvement to the above scheme, based on the closed-chain equation constraint, and comprehensively considering the robot's dual-arm input energy, reward term, and pose error between the end effector and the manipulated object, a multi-constraint coordinated motion planning optimization model for the dual-arm robot is constructed, as follows:

[0025]

[0026] stx t＝0 =x0,

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

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

[0029] Where V = {v0, v1, ..., v T-1} is the input sequence; t∈{0,1,...T-1} is the length of the prediction time domain;

[0030] s(x t ) is state x t The immediate cost; This is used to penalize the control input's sensitivity to noise, where λ is the regularization coefficient, used to balance control efficiency and noise sensitivity. It is the transpose of the control input, ∑ -1 It is the inverse of the noise covariance matrix. It is noise or random disturbance, usually assumed to be Gaussian noise.

[0031] x t＝0 =x0 is the initial state, x t+1 =F(x) t ,v t ) is the dynamic equation of the system, which can be solved by Pinocchio.

[0032] h(t,x t The equation ) = 0 represents a closed-chain equality constraint, and an appropriate cost s(x) will be added. t )middle.

[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 state cost of the input sequence in the sampling distribution.

[0034] As a further improvement to the above scheme, the information theory-based model prediction path integral control method in step S3 randomly samples the predicted trajectory using a Gaussian noise model and analyzes the error between the predicted trajectory sample and the expected trajectory using the KL divergence theory in information theory. Then, the importance sampling method is used to assign different weights to the predicted trajectory sample for calculation, and the optimized trajectory of the two-arm coordinated motion that satisfies multiple constraints is obtained by summing the samples.

[0035] As a further improvement to the above scheme, the information theory-based model prediction path integral control method utilizes the robot's initial state x0 and sampled input v t An importance sampling trajectory H is generated. Then, multiple constraints, such as task constraints, energy constraints, and end-effector position closed-chain constraints, are added to the trajectory cost C(V) calculation based on H. Finally, the robot performs optimization updates to obtain the optimal solution to the multi-constraint optimization problem. Then, it passes the control input sequence U and the initial state x0 to the next optimization update loop, as follows:

[0036]

[0037] Where: K represents the number of predicted trajectory samples; w(ε) k The weights of each trajectory prediction sample obtained through the importance sampling method are: U = {u0, u1, ..., u...} T-1};

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

[0039] The beneficial effects of this invention are:

[0040] Compared with existing technologies, this invention provides a multi-constraint dual-arm robot coordinated motion planning method. The coordinated motion of the two arms requires the construction of various forms of constraints, including task constraints, end-effector position closed-chain constraints, energy constraints, etc. The method introduces a quadratic penalty function to transform the end-effector position closed-chain equation constraint relationship in the dual-arm coordinated operation task under different scenarios into the objective function, and combines various forms of constraints to construct the optimization problem of the dual-arm coordinated operation task. The stochastic optimal dual-arm coordinated motion planning strategy based on the model prediction path integral control method can handle continuous or discontinuous cost functions to solve the high-dimensional motion coordination optimization problem of dual arms.

[0041] In summary, the coordinated motion planning method for dual-arm robots under multiple constraints provided by this invention offers a new theoretical and methodological foundation for the practical application of dual-arm robots in complex scenarios. Attached Figure Description

[0042] Figure 1 This is a schematic diagram of the process of the present invention;

[0043] Figure 2 This is a flowchart illustrating each step in the present invention. Detailed Implementation

[0044] The specific embodiments of the present invention will be further described in detail below with reference to the accompanying drawings:

[0045] Example 1

[0046] according to Figures 1-2 As shown, this invention provides a method for coordinated motion planning of a dual-arm robot under multiple constraints, comprising the following steps:

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

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

[0049] S2. Based on the requirements and objectives of the coordinated operation task of the dual-arm robot, a mathematical model of task constraints is established. The quadratic penalty function method is introduced to transform the end-position closed-chain equation constraints in the coordinated operation task of the dual-arm robot under different scenarios into the objective function, and a multi-constraint optimization problem of coordinated motion planning of dual-arm robot is constructed.

[0050] Among them, the coordinated operation tasks of dual-arm robots mainly include coordinated operation tasks of dual arms in Cartesian space, such as... Figure 2As shown in .a. The constraints for dual-arm coordinated operation tasks in Cartesian space mainly concern dual-arm closed-chain constraints, which indicate that the relative pose between the end effectors of the two arms remains unchanged during the motion after grasping the object.

[0051] The closed-chain equality constraint is given by h(t, x). t ) = 0 indicates that the specific form is as follows:

[0052]

[0053] Among them, ||q r -q l ||2=0 indicates that the Euclidean distance between the attitudes of the robot's right arm end effector and the left arm end effector is zero; The Euclidean distance between the position of the robot's right arm end effector to the target point on the right and the position of the left arm end effector to the target point on the left is zero.

[0054] The quadratic penalty function method is introduced to transform the closed-chain equality constraint into a soft constraint, forming an augmented cost function, the expression of which is as follows:

[0055]

[0056] Among them, ||q r -q l ||2 represents the Euclidean distance between the attitudes of the robot's right arm end effector and the left arm end effector; represents the Euclidean distance between the position of the robot's right arm end effector to the target point on the right and the position of the left arm end effector to the target point on the left. w6 and w7 are weighting coefficients used to balance the impact of the quadratic penalty term on the total loss.

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

[0058]

[0059] Among them, w1, w2, w3, and w4 are weighting coefficients, which are used to balance the impact of each penalty term on the total loss.

[0060] This indicates the various penalty items, specifically... This represents the Euclidean distance between the robot's right arm end effector and the target point on the right side; This represents the Euclidean distance between the robot's left arm end effector and the target point on the left side; The Euclidean distance between the robot's right arm end effector and the target point on the right side is represented by a quaternion. This represents the Euclidean distance between the robot's left arm end effector and the target point on the left side;

[0061] k energy This represents an energy term, typically used to penalize excessive speed to ensure safe arm movements, usually expressed as v. 2 The form is represented as , where v represents the sampled input;

[0062] e r and e l This represents the reward term, used to verify that the model's path integral control algorithm can handle discontinuous cost functions. The expression is as follows:

[0063]

[0064] Based on the closed-chain equation constraint, and comprehensively considering the input energy of the robot's two arms, the reward term, and the pose error between the end effector and the manipulated object, a multi-constraint coordinated motion planning optimization model for the dual-arm robot is constructed, such as... Figure 2 As shown in .b. Specifically:

[0065]

[0066] stx t＝0 =x0,

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

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

[0069] Where V = {v0, v1, ..., v} T-1} is the input sequence; t∈{0,1,...T-1} is the length of the prediction time domain;

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

[0071] x t＝0 =x0 is the initial state, x t+1 =F(x) t v t) is the dynamic equation of the system, which can be solved by Pinocchio.

[0072] h(t, x) t The equation ) = 0 represents a closed-chain equality constraint, and an appropriate cost s(x) will be added. t )middle.

[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 multi-constraint optimization problems, uses an information-theoretic model prediction path integral control method to obtain the optimal trajectory for coordinated bi-arm motion that meets the requirements of multi-constraint tasks, such as... Figure 2 As shown in .c.

[0075] Among them, the model prediction path integral control method based on information theory randomly samples the predicted trajectory using a Gaussian noise model and uses the KL divergence theory in information theory to analyze the error between the predicted trajectory sample and the expected trajectory. Then, the importance sampling method is used to assign different weights to the predicted trajectory sample and calculate the optimal trajectory of the two-arm coordinated motion that satisfies multiple constraints by summing the samples.

[0076] In the information theory-based model prediction path integral control method, the core model content is as follows:

[0077]

[0078] Among them, S(V k ) represents the cost of the sampling trajectory, where It is the terminal cost, representing the cost of the final state x. T The punishment; s(x) t ) represents state x t The immediate cost; the terminal cost in coordinated bi-arm movements is the cost of target deviation, already included in s(x t In ) so Specifically, it represents the cumulative cost of the kth trajectory sample from t=0 to T-1;

[0079] This is used to penalize the control input's sensitivity to noise, where λ is a regularization parameter used to balance control efficiency and noise sensitivity. It is the transpose of the control input, ∑ -1 It is the inverse of the noise covariance matrix. It is noise or random disturbance, usually assumed to be Gaussian noise.

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

[0081] The information-theory-based model predictive path integral control method utilizes the robot's initial state x0 and sampled input v t An importance sampling trajectory H is generated. Then, multiple constraints, such as task constraints, energy constraints, and end-effector position closed-chain constraints, are added to the trajectory cost C(V) calculation based on H. Finally, the robot performs optimization and updates the result to obtain the optimal solution to the multi-constraint optimization problem. Then, it passes the control input sequence U and the initial state x0 to the next optimization update loop, as follows:

[0082]

[0083] Where: K represents the number of predicted trajectory samples; w(ε) k The weights of each trajectory prediction sample obtained through the importance sampling method are: U = {u0, u1, ... u2}. T-1};

[0084] Sampling input Among them This represents the Gaussian white noise model.

[0085] S4 applies the optimal trajectory of coordinated bi-arm movement to the simulation operation task of the bi-arm robot.

[0086] This invention acquires robot target information and the robot's current state; based on the task requirements and objectives of the dual-arm robot's coordinated operation, it establishes a mathematical model of task constraints, constructing a multi-constraint optimization problem for the coordinated motion planning of the dual-arm robot; for the multi-constraint optimization problem, it employs an information theory-based model prediction path integral control method to solve it, obtaining the optimal trajectory of the coordinated motion of the dual arms that satisfies the multi-constraint task requirements; and applies the optimal trajectory of the coordinated motion of the dual arms to the robot's simulated operation task. This invention's method for dual-arm coordinated motion planning under multiple constraints improves the efficiency and stability of dual-arm robots in completing dexterous coordinated motion planning tasks, providing new theories and methods for the practical application of robot 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 modifications or equivalent substitutions that do not depart from the spirit and scope of the present invention should be covered within the scope of 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, acquire the target information and current state of the dual-arm robot; the target information is the pose of the object being manipulated; the current state includes the angles of each joint of the dual-arm robot, the control actions, and the Cartesian space pose of the end effectors of the dual arms; S2. Based on the requirements and objectives of the dual-arm robot coordinated operation task, a mathematical model of task constraints is established. The quadratic penalty function method is introduced to transform the end-effector position closed-chain equation constraints in the dual-arm robot coordinated operation task under different scenarios into the objective function, thus constructing a multi-constraint optimization problem for dual-arm robot coordinated motion planning. The dual-arm robot coordinated operation task mainly includes dual-arm coordinated operation tasks in Cartesian space. The constraints of dual-arm coordinated operation tasks in Cartesian space are mainly aimed at dual-arm closed-chain constraints, which indicate that the relative pose between the end effectors of the two arms remains unchanged during the motion process after grasping the object. The quadratic penalty function method is introduced to transform the closed-chain equality constraint into a soft constraint, forming an augmented cost function, the expression of which is as follows: ； in, This represents the Euclidean distance between the attitudes of the robot's right and left arm end effectors. represents the Euclidean distance between the position of the robot's right arm end effector to the target point on the right and the position of the left arm end effector to the target point on the left; w6 and w7 are weighting coefficients used to balance the impact of the quadratic penalty term on the total loss; The closed-chain equality constraint is... The specific form is as follows: ； in, This indicates that the Euclidean distance between the attitudes of the robot's right and left arm end effectors is zero. The Euclidean distance between the position of the robot's right arm end effector to the target point on the right and the position of the left arm end effector to the target point on the left is zero. The augmented cost function The expression is as follows: ； Among them, w1, w2, w3, w4, and w5 are weighting coefficients, which are used to balance the impact of each penalty term on the total loss. , , , This indicates the various penalty items, specifically... This represents the Euclidean distance between the robot's right arm end effector and the target point on the right side; This represents the Euclidean distance between the robot's left arm end effector and the target point on the left side; The Euclidean distance between the robot's right arm end effector and the target point on the right side is represented by a quaternion. This represents the Euclidean distance between the robot's left arm end effector and the target point on the left side; This represents an energy level, typically used to penalize excessive speed to ensure safe arm movements, usually expressed as... The form of representation, in which Indicates the sampling input; and The reward term is used to verify that the model's path integral control algorithm can handle discontinuous cost functions. Its expression is as follows: ； S3. For multi-constraint optimization problems, the model prediction path integral control method based on information theory is used to solve the problem and obtain the optimal trajectory of the coordinated movement of the two arms that meets the requirements of multi-constraint tasks. S4 applies the optimal trajectory of coordinated bi-arm movement to the simulation operation task of the bi-arm robot.

2. The method for coordinated motion planning of a dual-arm robot under multiple constraints according to claim 1, characterized in that: Based on the aforementioned closed-chain equation constraints, and comprehensively considering the robot's dual-arm input energy, reward term, and pose error between the end effector and the manipulated object, a multi-constraint coordinated motion planning optimization model for the dual-arm robot is constructed as follows: ， ， ， ； in, It is the input sequence; It is the length of the predicted time domain; It is a state The immediate cost; Used to penalize the sensitivity of the control input to noise, among which It is a regularization coefficient used to balance control efficiency and noise sensitivity. It is the transpose of the control input. It is the inverse of the noise covariance matrix. It is noise or random disturbance, usually assumed to be Gaussian noise. ; It is the initial state. These are the system's dynamic equations, which can be solved using Pinocchio. It is a closed-chain equality constraint, and an appropriate cost will be added. middle.

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

4. The method for coordinated motion planning of a dual-arm robot under multiple constraints according to claim 1, characterized in that: In step S3, the information theory-based model prediction path integral control method randomly samples the predicted trajectory using a Gaussian noise model and analyzes the error between the predicted trajectory sample and the desired trajectory using the KL divergence theory in information theory. Then, the importance sampling method is used to assign different weights to the predicted trajectory sample for calculation, and the optimized trajectory of the two-arm coordinated motion that satisfies multiple constraints is obtained by summing the samples.

5. The method for coordinated motion planning of a dual-arm robot under multiple constraints according to claim 4, characterized in that: The information-theory-based model prediction path integral control method utilizes the robot's initial state. and sampling input Generate importance sampling trajectory Then based on Trajectory cost The calculation incorporates multiple constraints, including task constraints related to the coordinated movement of both arms, energy constraints, and closed-loop constraints at the end-effector position. Finally, the optimal solution to the multi-constraint optimization problem is obtained by optimizing and updating the robot's performance. and control the input sequence and initial state Passed to the next optimization update loop, as follows: ； Where: K represents the number of predicted trajectory samples; The weights of each trajectory prediction sample obtained through the importance sampling method are used to control the input sequence. ; Sampling input , among them , representing the Gaussian white noise model.

Images