An assembly task optimization control method based on contact force constraint and posture constraint

Abstract

The application discloses an assembly task optimization control method based on contact force constraint and posture constraint, utilizes a control barrier function to constrain the contact force, allows the robot to control the applied force within a safe range when contacting the workpiece, thereby avoiding damaging sensitive components and ensuring the stability of assembly, and adds relevant equation constraints for the robot end effector posture in the optimization problem, ensures that the movement of the robot follows the predetermined posture for assembly, ensures the assembly precision and consistency, and through the combination of the two constraints, the contact force can be safely applied while maintaining the posture meeting the task expectation, which is favorable for improving the sticking problem of equipment under complex conditions, improves the assembly quality and safety, and promotes the application of the robot in contact force related tasks under more complex conditions.

Description

Technical Field

[0001] This invention belongs to the field of robot control technology, specifically relating to an assembly task optimization control method based on contact force constraints and posture constraints. Background Technology

[0002] In modern manufacturing, assembly is a crucial part of the production process, and its accuracy and efficiency directly affect product quality and cost. With the development of industrial automation, robots are increasingly being used in assembly tasks.

[0003] To accomplish assembly tasks, force control methods are required for robots, leading to the widespread application of robot impedance control technology. By adjusting the interaction forces between the robot and its environment, it enables the robot to complete tasks flexibly and efficiently in uncertain and dynamic environments. Impedance control improves the accuracy and quality of the assembly process, enhances the safety of human-robot collaboration, and promotes the automation of complex assembly tasks. Through this technology, robots can better adapt to diverse assembly requirements, significantly improving the efficiency and automation level of production lines. Further improvements to impedance control have become an important research direction.

[0004] However, achieving zero contact force error through impedance control has stringent conditions. Using optimized control algorithms to ensure that the contact force is within a safe range during assembly is a feasible method, but the optimal control quantity obtained through optimization may disrupt the robot's posture, making it difficult for the system to continue the assembly operation. Summary of the Invention

[0005] In view of this, the present invention proposes an assembly task optimization control method based on contact force constraints and posture constraints, so as to achieve contact force-related optimization control while avoiding the adverse effects of optimization problems on robot posture during assembly.

[0006] This invention proposes an assembly task optimization control method based on contact force constraints and attitude constraints, comprising the following steps:

[0007] Step 1: Establish the end-joint torque PD controller of the controlled robot using a PD controller with gravity compensation, and establish a contact force model for the controlled robot;

[0008] Step 2: Based on the target point reference position information of the controlled robot, use a compliant controller to calculate the desired angle q of the end joint of the controlled robot. d With desired angular velocity

[0009] Step 3: Based on the control obstacle function theory, establish the control obstacle function of the controlled robot for the contact force, and establish the equality constraints of the controlled robot with respect to its posture, as shown in the following equation:

[0010]

[0011] Among them, J ω The last three rows of the Jacobian matrix for the controlled robot For safety control purposes;

[0012] Step 4: Based on the control obstacle function and equality constraints established in Step 3, construct a quadratic programming optimization problem with the desired angular velocity as the optimization objective, and obtain the modified safety control quantity by solving this quadratic programming optimization problem;

[0013] Step 5: Integrate the safety control quantity obtained in Step 4 to obtain the safety angle. Input the safety angle and safety control quantity into the end joint torque PD controller to complete the control of the controlled robot.

[0014] Furthermore, the contact force model in step 1 is as follows:

[0015]

[0016] in, Let represent the stiffness characteristics of the controlled robot, and p represent the angle of the end effector of the controlled robot.

[0017] Furthermore, the control barrier function in step 3 is:

[0018]

[0019] in, Let α(h(x)) be the derivative of h(x), and let α(h(x)) be a continuously increasing monotonically increasing Lipschitz function with α(0) = 0; h(x) = F max -|F ext | is the absolute value of the difference between the maximum permissible contact force and the actual contact force, used to describe whether the system is safe.

[0020] Furthermore, the quadratic programming optimization problem in step 4 is:

[0021]

[0022] Among them, J QP Let L be the cost function of the quadratic programming. f h(x), L g h(x) is the Lie derivative of h(x). This is the minimum value of the safety control quantity. This represents the maximum value of the safety control quantity.

[0023] Furthermore, the compliant controller in step 2 is implemented using an impedance controller established by position-based impedance control.

[0024] Beneficial effects:

[0025] This invention utilizes a control barrier function to constrain contact forces, allowing the robot to control the applied force within a safe range when contacting the workpiece, thereby avoiding damage to sensitive components and ensuring assembly stability. Furthermore, it adds relevant equality constraints to the robot's end effector posture in the optimization problem, ensuring that the robot's motion follows a predetermined posture during assembly, guaranteeing assembly accuracy and consistency. By combining these two constraints, the robot can safely apply contact forces while maintaining a posture consistent with the task's expectations, which helps to improve the problem of equipment jamming under complex conditions. This improved impedance control method not only enhances assembly quality and safety but also promotes the application of robots in contact force-related tasks under more complex conditions. Attached Figure Description

[0026] Figure 1 This is a schematic diagram of the control task in an embodiment of an assembly task optimization control method based on contact force constraints and attitude constraints provided by the present invention.

[0027] Figure 2 This is a detailed parameter diagram of the UR3 robot used in an embodiment of an assembly task optimization control method based on contact force constraints and attitude constraints provided by the present invention.

[0028] Figure 3 This is a schematic diagram showing the change in contact force during the assembly process obtained by using only impedance control in the embodiment.

[0029] Figure 4 This is a schematic diagram illustrating the changes in contact force during the assembly process, obtained using an assembly task optimization control method based on contact force constraints and attitude constraints provided by the present invention in this embodiment.

[0030] Figure 5 This is a schematic diagram showing the changes in posture during the assembly process obtained by using only impedance control in the embodiment.

[0031] Figure 6 This is a schematic diagram showing the changes in posture during the assembly process obtained by using an assembly task optimization control method based on contact force constraints and posture constraints provided by the present invention in this embodiment. Detailed Implementation

[0032] The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0033] Equality constraints, as a fundamental function of optimization controllers, can precisely adjust the relationship between optimization variables. However, introducing equality constraints into complex tasks can easily lead to questions about the existence of solutions, thus limiting their application. This invention, based on an analysis of whether optimization problems have solutions after introducing equality constraints, determines a technical solution that incorporates attitude-related equality constraints.

[0034] This invention provides an assembly task optimization control method based on contact force constraints and attitude constraints, which mainly includes the following steps:

[0035] Step 1: Establish the contact force model of the controlled robot.

[0036] This invention introduces a gravity-compensated end-joint torque PD controller for the robot's underlying structure:

[0037]

[0038] Where, τ J ∈R n K represents the control torque of n joints. p ,K D For the parameters of the PD controller, Let represent the joint velocities and joint accelerations of the robot's n joints, respectively. Let represent the expected joint velocities and expected joint accelerations of n joints of a robot.

[0039] A contact force model is established for the controlled robot, as shown in the following equation:

[0040]

[0041] in, p,p represents the stiffness characteristics of the robot. d Let J(q) represent the position and desired position of the robot's end effector, respectively, and J(q) be the robot's Jacobian matrix.

[0042] Step 2: Based on the predetermined target point reference position information of the controlled robot, the desired angle q of the end joint of the controlled robot is calculated using a compliant controller. d With desired angular velocity The expected angle q d With desired angular velocity Substitute it into the PD controller.

[0043] Step 3: Based on the control obstacle function theory, establish the control obstacle function of the controlled robot for the contact force, as shown in the following formula:

[0044]

[0045] in, Let α(h(x)) denote the derivative of h(x), and let α(h(x)) denote the Lipschitz continuous monotonically increasing function, with α(0) = 0; h(x) = F max -|F ext | is the absolute value of the difference between the maximum permissible contact force and the actual contact force, used to describe whether the system is safe.

[0046] Step 4: Establish the equation constraints for the controlled robot's posture, as shown in the following equation:

[0047]

[0048] Among them, J ω The last three rows of the Jacobian matrix for the robot. For safety control purposes.

[0049] Step 5: Based on the control obstacle function established in Step 3 and the equality constraints established in Step 4, construct a quadratic programming optimization problem with the desired angular velocity as the optimization objective, as shown in the following equation:

[0050]

[0051] Among them, J QP Let L be the cost function of the quadratic programming. f h(x), L g h(x) are all Lie derivatives of h(x). This is the minimum value of the safety control quantity. This represents the maximum value of the safety control quantity.

[0052] The modified safety control quantity is obtained by solving this quadratic programming optimization problem.

[0053] Step 6: Integrate the safety control quantity obtained in Step 5 to obtain the safety angle. Use the safety angle and the safety control quantity together as the input to the PD controller in Step 1 to complete the control of the controlled robot.

[0054] In existing technologies, optimization control methods often encounter situations where the optimization problem becomes unsolvable, leading to control system failure. To address this issue, this invention proposes an optimization control method that combines safety constraints and attitude constraints, ensuring that a solution is always available, based on the following analysis and proof. The specific analysis and proof process is as follows:

[0055] First, when physical constraints When the problem does not exist, it can be known from existing technologies that the quadratic programming optimization problem constructed by this invention always has a solution. However, considering the existence of physical constraints, since... A smaller safety control amount can always be found. make To maintain the contact force within a safe range, the optimization problem still has a solution.

[0056] Then, consider introducing the equality constraints established by this invention. The impact on the optimization problem can be seen by reviewing the robot's dynamics:

[0057]

[0058] in

[0059] Obviously It is J ω Any vector in the null space of can be represented as:

[0060]

[0061] in For J ω A basis in the null space, where c is any three-dimensional vector.

[0062] In turn, one can obtain Based on the contact force model established for the controlled robot, we have:

[0063]

[0064] At this point, a method is provided that allows the system to remain within a safe range. It suffices to prove that c satisfies the above equation. The left-hand side of the equation consists of known quantities, and the right-hand side can be solved analytically since J(q). It can also be solved analytically. After finding the analytical solution using mathematical tools, it can be discovered that... Since the equation is full rank, it has a solution. We can always find a c such that the equation holds true; there always exists a c. This allows the system to remain within a safe region, thus the quadratic programming optimization problem constructed in this invention has a solution.

[0065] Example:

[0066] In this embodiment, an assembly task optimization control method based on contact force constraints and attitude constraints provided by the present invention is adopted, and a six-degree-of-freedom robot UR3 and a multi-axis hole assembly task are selected as the research object.

[0067] This embodiment enables fine assembly of biaxial holes with a gap of less than 0.5mm and a depth of more than 100mm under certain initial positional deviations (positional error of 10mm horizontally and attitude error of 5°). The goal is to significantly reduce contact forces during assembly (by at least 30%) after using optimized control, without affecting the normal completion of the assembly process. A schematic diagram of the control task is shown below. Figure 1 As shown, the detailed parameters of the UR3 robot used are as follows: Figure 2 As shown. The specific control process includes:

[0068] S1. Introduce or identify the robot's underlying PD controller, design or obtain its PD parameters, and establish a contact force model. Set the parameters of the joint torque PD controller to K. P =300I,K D =50I, thus obtaining the contact force model as follows:

[0069]

[0070] In the above formula, besides the input All other variables are known or can be represented by the joint angle q.

[0071] S2. Using classic position-based impedance control, an impedance controller is established to achieve the basic task of compliant assembly.

[0072] The basic formula for location-based impedance control is as follows:

[0073]

[0074] The above formula represents the impedance formula in one direction. In this embodiment, the above formula is used in the x, y and z directions of Cartesian space to achieve impedance control in Cartesian space. Here, we will only take impedance control in a certain direction as an example for explanation.

[0075] Based on the fundamental impedance formula, and adhering to the principle of avoiding differentiation of variables, we select... The controller, as a control variable, takes the following form:

[0076]

[0077] Integrating the above equation yields the velocity-related quantity:

[0078]

[0079] Further, angular velocity-related quantities are obtained through inverse kinematics. Combination The desired angular velocity can be obtained. After integration, the desired angle q can be obtained. d Both are used as inputs to the PD controller in embodiment S1 to complete the basic compliant assembly task.

[0080] S3. Design CBF contact force constraints to achieve constraint control of contact forces during assembly.

[0081] In this embodiment, S2 is considered to be used in the completion Figure 1During the basic assembly task shown, the maximum vertical contact force during the assembly process is approximately 15N. Therefore, the safe range for contact force is specified as no more than 10N in the vertical direction.

[0082] Based on the CBF theory, the corresponding h(x) is designed, and considering the safety range, we have:

[0083] h(x) = 10N - F z

[0084] Where F z This represents the vertical component of the contact force.

[0085] Based on the design h(x), the following CBF constraints can be obtained:

[0086]

[0087] in, Let α(h(x)) represent the derivative of h(x). In this embodiment, we choose α(h(x)) = 10·h(x).

[0088] S4. Design attitude constraints to prevent the robot end effector's attitude from being disrupted by the optimization controller.

[0089] Design the following equality constraints:

[0090] ω safe =ω d

[0091] in J ω The last three rows of the Jacobian matrix for the robot.

[0092] because

[0093]

[0094] It can be seen that as long as we maintain This allows the robot's end effector posture to change according to the control quantity generated by impedance control, and optimized control will not affect the end effector posture.

[0095] S5. Solve the optimization problem to obtain the safety control quantities that enable the system to satisfy safety and attitude constraints.

[0096] Combining S3 and S4, and taking the desired angular velocity as the optimization objective, we obtain the following optimization problem:

[0097]

[0098] ω safe =ω d

[0099] The modified, safe control quantity is obtained by solving this quadratic programming problem.

[0100] S6. Control the robot.

[0101] The safe angular velocity obtained from S5 Integrate to obtain the safe angle q safe Replacing the PD controller in S1 respectively q d This allows for the acquisition of control torque to control the robot system, achieving the goal of significantly reducing contact forces during compliant assembly.

[0102] Experimental results

[0103] A simulation experiment was conducted using the steps and parameters described in the embodiments, and the contact force results are shown in the figure below. Figure 3 and Figure 4 As shown, where, Figure 3 This shows the changes in contact force during the assembly process obtained using only impedance control. Figure 4 The figure shows the change in contact force during the assembly process obtained by using the assembly task optimization control method based on contact force constraints and attitude constraints provided by the present invention. As can be seen from the comparison of the two figures, the contact force during the assembly process is significantly reduced after using the optimization control algorithm provided by the present invention. The maximum contact force is reduced from about 14N to about 5N, thus achieving the task objective.

[0104] The workpiece posture error in the same experiment is as follows: Figure 5 and Figure 6 As shown, where, Figure 5 This shows the changes in posture during the assembly process obtained using only impedance control. Figure 6 The two figures show the changes in posture during the assembly process obtained by using the assembly task optimization control method based on contact force constraints and posture constraints provided by the present invention. As can be seen from the comparison of the two figures, the robot can still effectively adjust its posture after using the optimization control algorithm provided by the present invention, thereby reducing the posture error and finally successfully inserting it into the hole. The contact force constraint did not affect the ability of impedance control to adjust the posture.

[0105] In summary, the above two points demonstrate that the present invention can effectively reduce contact force during assembly while precisely adjusting the workpiece posture, and has a positive effect on solving the jamming problem in complex assembly situations.

[0106] In summary, the above are merely preferred embodiments of the present invention, and the accompanying drawings are only used to illustrate the effectiveness of the embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. An assembly task optimization control method based on contact force constraints and attitude constraints, characterized in that, Includes the following steps: Step 1: Establish the end-joint torque PD controller of the controlled robot using a PD controller with gravity compensation, and establish a contact force model for the controlled robot; Step 2: Based on the target point reference position information of the controlled robot, use a compliant controller to calculate the desired angle of the end effector joint of the controlled robot. With desired angular velocity ; Step 3: Based on the control obstacle function theory, establish the control obstacle function of the controlled robot for the contact force, and establish the equality constraints of the controlled robot with respect to its posture, as shown in the following equation: ， in, The last three rows of the Jacobian matrix for the controlled robot For safety control purposes; Step 4: Based on the control obstacle function and equality constraints established in Step 3, construct a quadratic programming optimization problem with the desired angular velocity as the optimization objective, and obtain the modified safety control quantity by solving this quadratic programming optimization problem; Step 5: Integrate the safety control quantity obtained in Step 4 to obtain the safety angle, and input the safety angle and safety control quantity into the end joint torque PD controller to complete the control of the controlled robot; The control barrier function in step 3 is: ， in, for The derivative of Let f be a continuously monotonically increasing function of Lipschitz, and ; The absolute value of the difference between the maximum permissible contact force and the actual contact force is used to describe whether the system is safe. The quadratic programming optimization problem in step 4 is: ， in, Let be the cost function of the quadratic programming. , for Lie derivative, This is the minimum value of the safety control quantity. This represents the maximum value of the safety control quantity.

2. The assembly task optimization control method according to claim 1, characterized in that, The contact force model in step 1 is: ， in, For the stiffness characteristics of the controlled robot, The angle of the end effector of the controlled robot.

3. The assembly task optimization control method according to claim 1, characterized in that, The compliant controller in step 2 is implemented using an impedance controller based on position-based impedance control.

Images