A six-degree-of-freedom hybrid mobility control method with motion accuracy and stability

Abstract

A six-degree-of-freedom hybrid admittance control method that combines motion accuracy and stability includes: measuring the contact force between the robot and the environment, and obtaining the actual contact force after zero-point correction, gravity and inertia compensation. b F e By judging | b F e |≤10 ‑3 The validity of the condition is determined by whether there is an external force input; if there is no external force input, i.e., | b F e |≤10 ‑3 When an open-loop six-degree-of-freedom admittance control is used, the calculated value from the previous control cycle is taken as the current state for the current cycle; when there is an external force input during human-machine interaction, i.e., | b F e |>10 ‑3 In this case, closed-loop six-degree-of-freedom admittance control is employed, using measured joint positions and velocities to calculate the current state and solve for admittance control. Beneficial technical effects include: enabling six-degree-of-freedom admittance control of position and attitude; achieving real-time following without external force and contact stability with external force; and smooth and stable switching between the presence and absence of external force.

Description

Technical Field

[0001] This invention relates to the field of compliant control and real-time tracking technology for human-robot interaction, and particularly to a six-degree-of-freedom hybrid admittance control method that combines motion accuracy and compliant control stability. Background Technology

[0002] In today's society, collaborative interaction between humans and robots has become an important research topic. During this interaction, if a robot only possesses positional movement capabilities without relevant contact force information, the safety of the operator cannot be guaranteed. Therefore, integrating force information into the robot control system—that is, achieving compliant robot control—has become an important research direction.

[0003] Compliance control is divided into passive compliance control and active compliance control. Passive compliance control achieves compliance by changing the elasticity of the mechanism (such as adding a spring at the end); while active compliance refers to achieving an active response of the robot to force through control strategies. Passive compliance methods are difficult to achieve precise control and have limited applicability. Research on active compliance control mainly includes admittance control, impedance control, and force-position hybrid control. Since most industrial robots currently lack torque control and only allow position control, and admittance control is ultimately controlled through the position inner loop, admittance control has wider applications.

[0004] In practical applications, when performing teleoperation or trajectory tracking, it is necessary to ensure both the real-time performance and accuracy of trajectory tracking when there is no external contact force, and to achieve compliant active avoidance characteristics when there is contact force. In admittance control, the deviation between the current position and the planned position needs to be substituted into the admittance control differential equation to solve for the target position in the next cycle. Since collaborative robots cannot reach the target position within a single control cycle, different estimates of the current robot position in the control strategy will produce different control effects. Summary of the Invention

[0005] This invention first derives the theory of six-degree-of-freedom admittance control, and then fully compares the impact of open-loop and closed-loop control on the admittance control of practical robots. Finally, it proposes a hybrid admittance control method that combines real-time tracking performance without contact force with control stability under contact force. Open-loop control is used when there is no contact force, by substituting the theoretical pose into the control system to achieve rapid convergence, thus achieving real-time and accurate trajectory tracking. Closed-loop control is used when there is contact force, by inputting the actual measured robot pose into the control system to achieve stability in force contact.

[0006] The beneficial technical effects of this invention include:

[0007] 1. It can achieve six-degree-of-freedom admittance control of position and attitude;

[0008] 2. Hybrid admittance control can achieve real-time following without external force and contact stability with external force;

[0009] 3. The proposed hybrid admittance control switches smoothly and stably in the presence or absence of external forces. Attached Figure Description

[0010] Figure 1 This is a schematic diagram of the admittance control system in an embodiment of the present invention;

[0011] Figure 2 This is a schematic diagram of the six-degree-of-freedom admittance control system model of an embodiment of the present invention, namely the mass-spring-damping model;

[0012] Figure 3 This is a schematic diagram of the planned pose and actual pose in the six-degree-of-freedom admittance control system of this invention.

[0013] Figure 4 This is a flowchart of a six-degree-of-freedom open-loop admittance control system.

[0014] Figure 5 It is the contact force and admittance deviation during open-loop control. Resulting image;

[0015] Figure 6 This is a flowchart of a six-degree-of-freedom closed-loop admittance control system.

[0016] Figure 7 The contact force and admittance deviation during closed-loop control Resulting image;

[0017] Figure 8 It is a six-degree-of-freedom hybrid admittance control flowchart that combines motion accuracy with compliant control stability;

[0018] Figure 9 This invention employs a hybrid admittance control method to manage contact force and admittance deviation. Result image. Detailed Implementation

[0019] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to specific embodiments and the accompanying drawings. However, those skilled in the art will understand that this invention is not limited to the accompanying drawings and the following embodiments.

[0020] In the description of the invention, it should be noted that directional terms such as "length," "width," "upper," "lower," "far," and "near" indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. These terms are used only for the convenience of describing the invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. They should not be construed as limiting the specific scope of protection of the invention. Furthermore, the terms "first" and "second" are used for descriptive purposes only to distinguish technical features and do not have substantive meaning. They should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features.

[0021] The six-degree-of-freedom hybrid admittance control method proposed in this embodiment of the invention includes four aspects: (1) derivation of the six-degree-of-freedom admittance control system, (2) open-loop six-degree-of-freedom admittance control, (3) closed-loop six-degree-of-freedom admittance control, and (4) six-degree-of-freedom hybrid admittance control that combines motion accuracy and compliant control stability. Each aspect is described in detail below.

[0022] (1) Derivation of a six-degree-of-freedom admittance control system

[0023] Admittance control systems are control systems that use input force and output position, such as... Figure 1 As shown, the force input from the environment is used to calculate the output trajectory through admittance control. The difference , and by Calculate the actual target position To control the robot's movement.

[0024] The model of the six-degree-of-freedom admittance control system in this embodiment of the invention is a mass-spring-damped model, such as... Figure 2 As shown.

[0025] The above mass-spring-damping model can be expressed by the following formula:

[0026]

[0027] in, The difference between the actual pose and the planned pose. They are respectively The first and second time derivatives. For the six-DOF admittance control system of this embodiment, position and attitude are discussed decoupled, as follows: Figure 3 As shown, with rotation matrix These represent the planned posture and the actual posture, respectively. These represent the planned location and the actual location, respectively. It can be obtained from the forward kinematics of the robotic arm. Due to the presence of external forces, there is a target difference between the planned pose and the actual pose. .

[0028] The decoupled six-degree-of-freedom admittance control formula is as follows:

[0029]

[0030] in For the corresponding control parameters, The external torque and external force in the end-effector coordinate system {T} correspond to the angle change and position change, respectively. The external force and external torque are obtained through the robot's end-effector six-dimensional force sensor. These represent the differences between the planned attitude and the actual attitude, respectively. The second and first derivatives with respect to time, These represent the differences between the planned location and the actual location, respectively. The second and first derivatives with respect to time.

[0031] Defined using exponential coordinates ,Right now:

[0032]

[0033]

[0034] in All are defined in the current end coordinate system {T}. First time derivative Angular velocity in the tool coordinate system There is a connection between them:

[0035]

[0036] in:

[0037]

[0038]

[0039] and Describing a skew-symmetric matrix

[0040]

[0041] From the above formula, we can derive:

[0042]

[0043] in These are the planned angular velocity and the actual angular velocity at the end point in the base coordinate system, respectively.

[0044]

[0045] have to First time derivative Represented as:

[0046]

[0047] First time derivative for:

[0048]

[0049] in These are the planned linear velocity and the actual linear velocity at the end point in the base coordinate system, respectively. It can be obtained from the joint angular velocity:

[0050]

[0051] in Let be the spatial Jacobian matrix of the robotic arm.

[0052] The planned pose can be obtained from the above derivation. The actual pose obtained from the forward kinematics of the robotic arm and their difference And through joint speed and Jacobi matrix Calculate the actual terminal angular velocity and linear velocity And then obtained from the formula .Will Substituting the decoupled six-degree-of-freedom admittance control formula, the second-order differential equation is solved by giving the variables and their first-order differential initial values ​​to obtain a new objective. By modifying the above formula:

[0053]

[0054]

[0055] Determine the target position of the robot's end effector The robot's joint angles are obtained through inverse kinematics. The robot is sent out, and it reaches the target position through its internal position controller to achieve admittance control.

[0056] (2) Open-loop six-degree-of-freedom admittance control

[0057] As can be deduced from (1), The calculation needs to be based on the current pose. Joint velocity With planned pose and planning speed The calculation yields different estimation methods for the current pose and velocity.

[0058] Open-loop six-DOF admittance control does not recalculate by obtaining the robot's joint positions and velocities. Instead, it directly uses the current period obtained by solving the second-order differential equation in the previous control cycle. The relevant value is used as the initial value for the current control cycle and then used for the next control cycle. Parameter calculation. This involves using open-loop control, without feedback, assuming the robot reaches the commanded position within each control cycle. What is sought at all times The target state at any given time is The actual state at any given moment.

[0059] Right now Time passes through initial value And measure environmental contact force Substituting into the second-order differential equation of admittance control

[0060]

[0061] Solving the equation yields Target offset at time And through formula (1)

[0062]

[0063]

[0064] Find the pose of the target robot The joint angles are obtained by solving inverse kinematics. This allows the robot to reach the target pose. For open-loop admittance control, in the next control cycle, the... Initial value calculation at time step When determining the target position at time 1, the initial value is obtained using the theory from the previous period. Target offset at time rather than by acquiring actual Substituting the robot's joint angles and joint velocities into formula (1) at any given time, the initial values ​​are calculated as follows: Figure 4 As shown. Therefore, in the open-loop admittance control calculation, theoretical values ​​are used, and there is no need to measure the actual values; that is, the target position is assumed to be the actual position.

[0065] Experimental results are as follows Figure 5 As shown in the figure. The top figure shows the change of external force over time, and the bottom figure shows the admittance control offset. A graph showing changes over time. (Example) Figure 5Divide the graph into two parts, left and right. The left half has no external force or torque. At this time, the difference between the target position output by the admittance controller and the planned position is... It is also 0, meaning that at this point the path will be followed exactly as planned. Movement. The right half shows the external force and torque when in contact with a person. The results show that open-loop control exhibits oscillating characteristics when human-machine interaction is involved. Therefore, it can be concluded that open-loop admittance control has good following performance and fast response when there is no external force, but it exhibits oscillations and instability during human-machine interaction.

[0066] (3) Closed-loop six-degree-of-freedom admittance control

[0067] The open-loop six-degree-of-freedom admittance control directly uses the differential equations obtained from solving the differential equations in the previous control cycle. As the initial conditions differ for each cycle, the closed-loop six-degree-of-freedom admittance control obtains the current joint position through the robot's internal encoder in each control cycle. and joint velocity The current pose is obtained through forward kinematics. and current speed Then, through formula (1)

[0068]

[0069] Calculated Substituting the initial values ​​into the second-order differential equation of the admittance control to obtain the target value for the next control cycle. Values, control processes such as Figure 6 As shown. Since a real robot cannot reach the target position within a single control cycle, the performance of closed-loop six-DOF admittance control differs from that of open-loop control.

[0070] Experimental results are as follows Figure 7 As shown in the figure. The top figure shows the change of external force over time, and the bottom figure shows the admittance control offset. The graph shows the changes over time. It is divided into two parts for analysis: the left half represents the case with no external force and no external torque input, while the right half represents the case with external force and torque input during human-computer interaction. Figure 7 It can be seen that when the external force and external torque are 0, there is an admittance control offset. , This offset is caused by motion errors resulting from the robot's inability to reach the target position within the control cycle, calculated through admittance control. Admittance control offset. , This will cause the movement to deviate from the planned path even without external contact. This results in decreased tracking accuracy. Figure 7As can be seen from the right half, unlike open-loop admittance control, closed-loop admittance control does not exhibit periodic oscillations in the image when there is an external force input during human-machine interaction. In other words, the contact is stable and no oscillations occur, thus exhibiting compliant control stability in human-machine interaction.

[0071] (4) Six-degree-of-freedom hybrid admittance control that combines motion accuracy and compliant control stability

[0072] Based on the summaries of (1)-(3), embodiments of the present invention propose a six-degree-of-freedom hybrid admittance control that combines motion accuracy with compliant control stability. The six-degree-of-freedom admittance control model is described in (1). Addressing the different effects of open-loop and closed-loop admittance control in (2) and (3), embodiments of the present invention propose switching between open-loop and closed-loop admittance control by determining the presence or absence of contact force, thereby achieving both excellent motion accuracy without contact force and stability during human-machine interaction. The flowchart is as follows. Figure 8 As shown, the contact force between the robot and the environment is measured using a six-dimensional force sensor, and the actual contact force is obtained after necessary zero-point correction, gravity and inertia compensation. By judgment The validity of the condition is determined by whether there is an external force input. If there is no external force input, then... When open-loop control is used, as described in (2), the calculated value of the previous control cycle is used as the current state of the current cycle, thereby ensuring the response speed and following accuracy when there is no external force; when there is an external force input in the human-machine interaction, i.e. When the time is right, closed-loop control is used, as described in (3), the current state is calculated based on the measured joint position and joint velocity to solve the admittance control, thereby ensuring the stability of the contact and avoiding oscillation.

[0073] Experimental results are as follows Figure 9 As shown, the upper graph shows the change of external force over time, and the lower graph shows the corresponding admittance control deviation. , The graph shows the change over time, divided into three parts: left, middle, and right. It can be seen that the input external force is 0 in the left half, and at this point... , It is also 0, meaning the control will follow the planned trajectory exactly. Movement, thus achieving high tracking performance; in the middle section, it can be observed that when the contact force is not zero, the admittance control deviation... , The changes were smooth, without any oscillations, indicating controlled stability in actual motion contact; and by Figure 9 The left, middle, and right sections show the process of switching between having and not having external force. , Smooth switching was also achieved, meaning the transition between the hybrid control methods was smooth. In summary, experiments show that the six-degree-of-freedom hybrid admittance control method provided in this embodiment of the invention exhibits no admittance control offset in the absence of external forces. , Thus, it exhibits the advantage of good following performance in (2); when there is external force contact, there is no oscillation effect, exhibiting the advantage of smooth human-machine contact and stable control in (3); at the same time, a smooth switch can be achieved between the two.

Claims

1. A six-degree-of-freedom hybrid admittance control method that combines motion accuracy and stability, characterized in that, The method includes: The contact force between the robot and its environment is measured using a six-dimensional force sensor, and the actual contact force is obtained after zero-point correction, gravity and inertia compensation. By judgment Whether the condition is valid depends on whether there is external force input; When there is no external force input, that is When an open-loop six-degree-of-freedom admittance control is used, the calculated value from the previous control cycle is taken as the current state for the current cycle; when there is an external force input during human-machine interaction, i.e. When the time is right, closed-loop six-degree-of-freedom admittance control is used, and the current state is calculated based on the measured joint position and joint velocity to solve the admittance control problem. Both the open-loop six-degree-of-freedom admittance control and the closed-loop six-degree-of-freedom admittance control are based on the derivation of the six-degree-of-freedom admittance control system. The derivation of the six-degree-of-freedom admittance control system includes: A six-degree-of-freedom admittance control system is a control system that takes force as input and outputs position. It takes force as input from the environment, and outputs the planned trajectory through admittance control calculations. The difference , and by Calculate the actual target position To control the robot's movement; The model of a six-degree-of-freedom admittance control system is a mass-spring-damped model, which can be expressed by the following formula: ； in, The difference between the actual pose and the planned pose. They are respectively The first and second time derivatives; for a six-DOF admittance control system, position and attitude are decoupled and discussed using a rotation matrix. These represent the planned posture and the actual posture, respectively. These represent the planned location and the actual location, respectively. It can be obtained from the forward kinematics of the robotic arm; due to the presence of external forces, there is a target difference between the planned pose and the actual pose. ; The decoupled six-degree-of-freedom admittance control formula is as follows: ； in For the corresponding control parameters, The external torque and external force in the end-effector coordinate system {T} correspond to the angle change and position change, respectively. The external force and external torque are obtained through the robot's end-effector six-dimensional force sensor. These represent the differences between the planned attitude and the actual attitude, respectively. The second and first derivatives with respect to time, These represent the differences between the planned location and the actual location, respectively. The second and first derivatives with respect to time; Defined using exponential coordinates ,Right now: ； in All are defined in the current end coordinate system {T}; First time derivative Angular velocity in the tool coordinate system There is a connection between them: ； in: ； and Describing a skew-symmetric matrix ； From the above formula, we can derive: ； in These are the planned angular velocity and the actual angular velocity at the end point in the base coordinate system, respectively. ； have to First time derivative Represented as: ； First time derivative for: ； in These are the planned linear velocity and the actual linear velocity at the end point in the base coordinate system, respectively. It can be obtained from the joint angular velocity: ； in The spatial Jacobian matrix of the robotic arm; The planned pose is obtained from the above derivation. The actual pose obtained from the forward kinematics of the robotic arm and their difference And through joint speed and Jacobi matrix Calculate the actual terminal angular velocity and linear velocity And then obtained from the formula ;Will Substituting the decoupled six-degree-of-freedom admittance control formula, the second-order differential equation is solved by giving the variables and their first-order differential initial values ​​to obtain a new objective. By modifying the above formula: ； Determine the target position of the robot's end effector The robot's joint angles are obtained through inverse kinematics. The robot is sent out, and it reaches the target position through its internal position controller to achieve admittance control.

2. The six-degree-of-freedom hybrid admittance control method with both motion accuracy and stability according to claim 1, characterized in that, The open-loop six-degree-of-freedom admittance control includes: The calculation needs to be based on the current pose. Joint velocity With planned pose and planning speed The calculation yields different estimation methods for the current pose and velocity; Open-loop six-DOF admittance control does not recalculate by obtaining the robot's joint positions and velocities. Instead, it directly uses the current period obtained by solving the second-order differential equation in the previous control cycle. The relevant value is used as the initial value for the current control cycle and then used for the next control cycle. The parameter calculation employs an open-loop control method, without feedback, assuming the robot reaches the commanded position within each control cycle. What is sought at all times The target state at any given time is Real-time status; Right now Time passes through initial value And measure environmental contact force. Substituting into the second-order differential equation of admittance control ； Solving the equation yields Target offset at time ,formula ； Find the pose of the target robot The joint angles are obtained by solving inverse kinematics. This means it can control the robot to reach the target pose; for open-loop admittance control, in the next control cycle, it is controlled by... Initial value calculation at time step When determining the target position at time 1, the initial value is obtained using the theory from the previous period. Target offset at time rather than by acquiring actual The initial values ​​of the robot's joint angles and joint velocities are calculated by substituting them into the formulas in the derivation of the six-degree-of-freedom admittance control system. In the open-loop six-degree-of-freedom admittance control calculation, theoretical values ​​are used, and there is no need to measure the actual values. That is, the target position is assumed to be the actual position.

3. The six-degree-of-freedom hybrid admittance control method with both motion accuracy and stability according to claim 1, characterized in that, The closed-loop six-degree-of-freedom admittance control includes: Closed-loop six-DOF admittance control obtains the current joint position through the robot's internal encoder in each control cycle. and joint velocity The current pose is obtained through forward kinematics. and current speed Subsequently, the formulas were derived using a six-degree-of-freedom admittance control system. ； Calculated Substituting the initial values ​​into the second-order differential equation of the admittance control to obtain the target value for the next control cycle. value.

Images