A robot contact force constraint control method based on control barrier function

Abstract

The application discloses a robot contact force constraint control method based on a control barrier function, and an affine nonlinear system model containing contact force is constructed based on a kinematics model, a dynamics model and a contact force model of the robot, which has better dynamic performance compared with a single contact force model, the control barrier function is applied to robot contact force safety constraint control, compared with existing position safety constraint control methods, the application of the control barrier function in the robot field is widened, the control barrier function can be combined with various existing force-position control methods, has good contact force constraint effect, high flexibility, good real-time performance, strong robustness, simple application, and effectively improves the problem that traditional force-position control is difficult to constrain contact force.

Description

Technical Field

[0001] This invention belongs to the field of robot control technology, specifically relating to a robot contact force constraint control method based on a control obstacle function. Background Technology

[0002] Robots, as important industrial automation equipment, have been widely used in various fields. Their flexibility and versatility make them an indispensable part of automated production. However, as robots are used more extensively in work scenarios, the requirements for their control and precision are also increasing. Simple position control is often insufficient to meet the diverse needs of production. Taking assembly tasks as an example, position-controlled robots cannot perceive the stress state of parts, which can easily lead to improper assembly of precision parts or even damage to the parts.

[0003] Currently, classic methods such as impedance control and force-position hybrid control are effective ways to achieve force control. Impedance control is better suited to force control problems in hard environments, while force-position hybrid control performs better in soft environments. However, both are prone to significant overshoot and oscillations, making it difficult to meet the requirements of precise control. While adjusting the environmental model can achieve a smoother control process for these traditional methods, it is difficult to achieve constraint control, and the safe operation of the system cannot be guaranteed solely by impedance control or force-position hybrid control. Summary of the Invention

[0004] In view of this, the present invention proposes a robot contact force constraint control method based on a control obstacle function. By modifying the desired velocity and combining it with existing control methods, the present invention achieves safe constraint control of robot contact force, thereby improving the problem of difficulty in implementing contact force constraints in existing robot control methods.

[0005] This invention provides a robot contact force constraint control method based on a control obstacle function, comprising the following steps:

[0006] Step 1: Establish the robot dynamics model of the controlled robot; determine the force-position control method based on the robot joint angular velocity, and design the corresponding controlled robot controller;

[0007] Step 2: Establish a contact force model for the interaction process of the controlled robot, and combine it with the robot dynamics model of the controlled robot to establish an affine nonlinear system model of the controlled robot.

[0008] Step 3: Design a control obstacle function for the contact force based on the affine nonlinear system model and the task requirements of the controlled robot, and design a safety controller based on the control obstacle function. The input of the safety controller is the output of the controlled robot controller and the current joint angle and current contact force of the controlled robot. The safety controller solves for the safe joint angular velocity of the controlled robot when the contact force constraint is satisfied, and then obtains the safe joint angle by integrating the safe joint angular velocity.

[0009] Step 4: Use the safe joint angular velocity and safe joint angle obtained in Step 3 to replace the desired angular velocity and desired angle of the controlled robot controller as the final control quantity of the controlled robot, so as to realize the contact force safety constraint control of the robot-environment interaction system.

[0010] Furthermore, the method for establishing the robot dynamics model of the controlled robot in step 1 is as follows:

[0011] Introducing a PD controller with gravity compensation:

[0012]

[0013] in, This represents the control torque of n joints. For the parameters of the PD controller, Let represent the joint angles and joint angular velocities of n joints of the controlled robot, respectively. Represents the expected joint angles and expected joint angular velocities of n joints of a robot;

[0014] Therefore, the robot dynamics model is established as follows:

[0015]

[0016] in, These represent the external forces acting on the end effector of the controlled robot. The Jacobian matrix of the controlled robot.

[0017] Furthermore, the force-position tracking control method of the controlled robot controller is impedance control, admittance control, or model predictive control.

[0018] Furthermore, the contact force model in step 2 is as follows:

[0019]

[0020] in, This represents the stiffness characteristics of the controlled robot. These represent the position and desired position of the end effector of the controlled robot, respectively.

[0021] Furthermore, the affine nonlinear system model in step 2 is as follows:

[0022]

[0023] in, For system state variables, To control the quantity.

[0024] Furthermore, in step 3, the safety controller solves for the safe joint angular velocity of the controlled robot when satisfying the contact force constraint by converting the control obstacle function into a quadratic programming problem for solution. The quadratic programming problem is as follows:

[0025]

[0026] in, Indicates the safe joint angular velocity. express The derivative, Let represent a continuously monotonically increasing function of Lipschitz, and This indicates the range of joint angular velocities allowed for the controlled robot.

[0027] Beneficial effects:

[0028] This invention constructs an affine nonlinear system model incorporating contact forces based on the established kinematic, dynamic, and contact force models of the robot. Compared to considering a single contact force model, it exhibits better dynamic performance. By applying the control obstacle function to the robot's contact force safety constraint control, it broadens the application of the control obstacle function in the robotics field compared to existing position safety constraint control methods. Furthermore, it can be combined with various existing force-position control methods, demonstrating excellent contact force constraint effects, high flexibility, good real-time performance, strong robustness, and ease of application. It effectively improves the problem of difficulty in contact force constraint in traditional force-position control. Attached Figure Description

[0029] Figure 1 The UR3 robot used in an embodiment of a robot contact force constraint control method based on a control obstacle function provided by the present invention.

[0030] Figure 2 The simulation results of the end effector when it is stationary in an embodiment of a robot contact force constraint control method based on a control obstacle function provided by the present invention.

[0031] Figure 3 The simulation results of the end effector during motion are shown in an embodiment of a robot contact force constraint control method based on a control obstacle function provided by the present invention. Detailed Implementation

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

[0033] Control barrier functions, as a safety control method, are gradually attracting attention due to their superior real-time performance compared to constraint control methods such as model predictive control or reference governors. Control barrier functions are an application of barrier functions in safety control design, transforming the safety control problem into a more manageable convex optimization problem through concepts such as safe state sets and unsafe state sets. This invention provides a robot contact force safety constraint control method based on control barrier functions. Its core idea lies in establishing an affine nonlinear system model of contact forces during robot-environment interaction, and using control barrier function theory to design control barrier functions for contact forces. This yields safe robot joint velocities that meet constraint control requirements, thus achieving safe constraint control of robot contact forces.

[0034] This invention provides a robot contact force constraint control method based on a robot model and a control obstacle function, which mainly includes the following steps:

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

[0036] This invention introduces a PD controller with gravity compensation:

[0037]

[0038] in, This represents the control torque of n joints. For the parameters of the PD controller, Let represent the joint angles and joint angular velocities of the robot's n joints, respectively. This represents the expected joint angles and expected joint angular velocities of n joints of a robot.

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

[0040]

[0041] in, The external force acting on the robot's end effector. Let be the Jacobian matrix of the robot.

[0042] Step 2: Determine the force-position control method based on the robot joint angular velocity, design the corresponding controlled robot controller, and use the desired joint angular velocity and desired joint angle output by the controlled robot controller to achieve position and force tracking control of the controlled robot.

[0043] This invention determines that force-position tracking control can employ impedance control, admittance control, and model predictive control, etc. Through such force-position tracking control, the desired joint angular velocity and desired joint angle that satisfy the force-position tracking control constraints can be solved.

[0044] Step 3: Establish a contact force model for the interaction process of the controlled robot. Based on this, and in conjunction with the robot dynamics model of the controlled robot, establish an affine nonlinear system model of the controlled robot.

[0045] The contact force model of the controlled robot established in this invention is as follows:

[0046]

[0047] in, This indicates the robot's stiffness characteristics. These represent the position and desired position of the robot's end effector, respectively.

[0048] The established affine nonlinear system model of the controlled robot is as follows:

[0049]

[0050] in, For system state variables, As a control variable, this model describes the dynamic and contact force characteristics of a robot with a PD controller interacting with its environment.

[0051] Step 4: Design a control barrier function for the contact force based on the affine nonlinear system model and the task requirements of the controlled robot. Then, design a safety controller based on the control barrier function. The inputs to the safety controller are the output of the controller from Step 2 and the current joint angle of the controlled robot. and current contact force The safety controller is used to solve the safe joint angular velocity of the controlled robot when the contact force constraint is satisfied, and then the safe joint angle is obtained by integrating the safe joint angular velocity.

[0052] The safety controller input is the current joint angle of the controlled robot. Current contact force Desired joint angle Desired joint angular velocity and the control obstacle function designed based on the contact force constraint target. The output is the safe joint angular velocity that meets the safety constraints. .

[0053] The above control process is solved using a quadratic programming problem, which is:

[0054]

[0055] in, express The derivative, Let represent a continuously monotonically increasing function of Lipschitz, and This indicates the range of joint angular velocities allowed for the robot.

[0056] Step 5: Use the safe joint angular velocity and safe joint angle obtained in Step 4 to replace the desired angular velocity and desired angle of the controller in Step 2 as the final control quantity of the controlled robot, so as to realize the contact force safety constraint control of the robot-environment interaction system.

[0057] Example:

[0058] This embodiment employs a robot contact force constraint control method based on a robot model and control obstacle function provided by the present invention. The force constraint control problem during the interaction between a six-DOF robot UR3 and a rigid plane is selected as the controlled object, and is described in detail below with reference to the accompanying drawings:

[0059] In this embodiment, the controlled robot is the UR3 robot, and its structure is as follows: Figure 1 As shown, the end effector of the controlled robot is stationary or moves horizontally along a plane while interacting with the rigid plane. The constraint control target for the contact force is that the absolute value of the contact force in the vertical direction is no greater than 4N.

[0060] To accomplish this task, the specific steps are as follows:

[0061] a. Introduce a PD controller to perform kinematic and dynamic modeling of the controlled robot and establish a robot dynamic model.

[0062] The kinematic model of the UR3 robot was obtained using the screw method, and a corresponding PD controller was designed to establish the robot's dynamic model.

[0063]

[0064] Selecting PD control parameters , .

[0065] b. Design a position and force tracking controller. In this embodiment, admittance control is used as the force and position control method based on the robot joint angular velocity.

[0066] In the direction of force interaction, i.e., the vertical direction, design admittance control:

[0067]

[0068] in, For inertial parameters, For damping parameters, For stiffness parameters, The contact force in that direction, These represent the position, velocity, and acceleration in that direction, respectively. These represent the reference position, reference velocity, and reference acceleration in that direction, respectively. This represents the desired contact force in this direction. To achieve better contact force control, the following parameters are selected: , , The unit of length is millimeters.

[0069] Define the correction amount for the reference position First, we need to solve for the corresponding acceleration correction:

[0070]

[0071] Then, the velocity correction and the reference position correction are solved by integration:

[0072]

[0073] This embodiment achieves impedance control by correcting the reference velocity, which is then converted into an angular velocity correction.

[0074]

[0075] in, for The representation in Cartesian space, This is the correction amount for the angular velocity. Combined with the reference angular velocity. Obtain the desired angular velocity .

[0076] c. Model the contact forces to obtain an affine nonlinear system model for the force interaction problem.

[0077] For rigid contact, it can be approximated that the end effector position remains constant in the direction of force interaction. Therefore, the control torque is used to generate the contact force, according to the PD controller:

[0078]

[0079] Using contact force instead of torque, we get:

[0080]

[0081] Because the end effector is approximately stationary, the position control error... The relationship between contact force and position error can be obtained:

[0082]

[0083] in, The stiffness characteristics of the robot's interaction with the environment are described.

[0084] Write the contact force in differential form:

[0085]

[0086] By combining the dynamic model, we can obtain the affine nonlinear system model for the force interaction problem of the robot system:

[0087]

[0088] d. Design control obstacle functions based on the constraint objectives and solve the corresponding quadratic programming problem.

[0089] The control barrier function is designed as follows:

[0090]

[0091] in, This is the vertical component of the contact force in the end effector coordinate system; it is generally negative during force interaction.

[0092] The safe speed is then solved using the following quadratic programming approach. :

[0093]

[0094] in , For parameters, , .

[0095] e. will Points obtained and use replace , replace The PD controller introduced in section a is used for tracking.

[0096] After solving for the safe speed using the above steps and tracking it with a PD controller, safe constraint control of the contact force can be achieved. Two sets of simulation experiments were conducted, and the tracking target and results are as follows:

[0097] In the first group, the end effector position remains constant, and the contact force is a sinusoidal function that varies with time. Different... The simulation results are as follows: Figure 2 As shown, the contact force is effectively constrained under the action of the controller, and by selecting a suitable... It can make the contact force have a repulsive effect on the safety boundary to varying degrees.

[0098] In the second group, the end effector moves along an Archimedean spiral, and the contact force remains constant. The simulation results are as follows: Figure 3 As shown, the contact force is basically maintained within the safe range during the motion, and the controller can still maintain the constraint of the contact force well when disturbed.

[0099] The above experiments verify the effectiveness of the method provided by the present invention, which can effectively constrain and control the contact force when the robot interacts with the environment in various situations, and can significantly improve the problem that some existing robot force and position control methods are difficult to achieve contact force constraint control.

[0100] 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. A robot contact force constraint control method based on a control barrier function, characterized by, The method comprises the following steps: Step 1, a robot dynamics model of a controlled robot is established; a force-position control mode based on joint angular velocity of the robot is determined, and a corresponding controlled robot controller is designed; Step 2, a contact force model of an interaction process of the controlled robot is established, and an affine nonlinear system model of the controlled robot is established in combination with the robot dynamics model of the controlled robot; Step 3, a control barrier function about the contact force is designed according to the affine nonlinear system model and a task demand of the controlled robot, and a safety controller is designed according to the control barrier function, wherein an input of the safety controller is an output of the controlled robot controller and a current joint angle and a current contact force of the controlled robot, the safety controller is used to solve a safe joint angular velocity of the controlled robot when the contact force constraint is met, and a safe joint angle is obtained by integration according to the safe joint angular velocity; Step 4, the safe joint angular velocity and the safe joint angle obtained in step 3 are used to replace a desired angular velocity and a desired angle of the controlled robot controller as a final control quantity of the controlled robot, so that a contact force safety constraint control of a robot and environment interaction system is realized. The robot dynamics model of the controlled robot in step 1 is established in the following manner: A PD controller with gravity compensation is introduced: ， wherein, denotes a control torque of n joints, are parameters of the PD controller, denote joint angles and joint angular velocities of n joints of the controlled robot, respectively, denote desired joint angles and desired joint angular velocities of n joints of the robot; Therefore, the robot dynamics model is established as: ， wherein, respectively represent external forces experienced by the end effector of the controlled robot, is the Jacobian matrix of the controlled robot; The contact force model in step 2 is: ， wherein, represents a stiffness characteristic of the controlled robot, respectively represent a position and a desired position of an end effector of the controlled robot.

2. The robot contact force constraint control method according to claim 1, wherein, The force-position control mode of the controlled robot controller is impedance control, admittance control or model predictive control.

3. The robot contact force constraint control method according to claim 1, wherein, The affine nonlinear system model in step 2 is: ， wherein is a system state quantity, is a control quantity.

4. The robot contact force constraint control method according to claim 1, wherein, The manner in which the safety controller solves the safe joint angular velocity of the controlled robot when the contact force constraint is met in step 3 is that the control barrier function is converted into a quadratic programming problem for solving, and the quadratic programming problem is: ， wherein denotes the safe joint angular velocity, denotes derivative of denotes a Lipschitz continuous, monotonically increasing function, and , denotes the range of joint angular velocities allowed by the controlled robot, is a control barrier function.

Images