A Force Guidance Control Method and System for Variable Admittance of a Robot

Abstract

The present invention discloses a force guidance control method and system with variable admittance for a robot, which relates to the technical field of robots. By acquiring the interaction force between the robot and a human, the direction of the human's motion intention is inferred in real time, and by adjusting the inertia parameter and damping parameter, the robot is made to conform to the motion, including the following steps: collecting the motion information of the robot end and the human-machine interaction force information, predicting the acceleration at the next moment by using the minimum jerk model and the speed curvature mode, and obtaining the desired force by combining the admittance control equation and the current speed; generating a virtual inertia and virtual damping adjustment strategy according to the direction of the desired force; obtaining the desired speed and angular velocity for the next control cycle according to the adjusted virtual inertia and virtual damping; controlling the robot end to perform a compliant action and rejecting external disturbances. The present invention does not require a preset trajectory and prerequisite conditions, improves the accuracy, smoothness and intuitiveness of the taught trajectory, improves the applicability of the human-machine interaction process, and can be applied to a variety of unknown scenarios.

Description

Technical Field

[0001] The present invention relates to the technical field of robots, and in particular, to a force guidance control method and system with variable admittance for robots. Background Art

[0002] Robots have the advantages of intelligence, high operation accuracy, high dexterity, etc., and are widely used in various fields such as assembly, remote operation, and robot rehabilitation. The applications in the above fields are inseparable from physical human-robot interaction. Direct teaching is a widely used function, where humans drag the end of the robot to generate a motion trajectory, providing a more intuitive and time-saving robot programming method. A reasonable compliance control method can not only ensure the safety and efficiency of the task, but also ensure that the initial trajectory obtained by robot teaching is more natural, smooth, and compliant.

[0003] Impedance control and admittance control are often used in robot teaching due to their active compliance. Impedance control is a control method that inputs displacement and outputs force. However, in the teaching scenario, since the force is generated by humans and the robot needs to output corresponding displacements to cooperate with the input of the force, the robot is modeled and controlled as an admittance in most scenarios. In addition, the admittance controller can more easily adapt to the current robot system mainly based on position control. In teaching without a preset desired path, although the robot can adapt to human hand operations under admittance control, in the face of some inevitable situations such as external disturbances and noise effects, when the trajectory deviates from the real-time planned path of humans, frequent error correction will make the teaching process non-compliant, discontinuous, and the error increases. Since the admittance control parameters in the existing control schemes are fixed or only consider the acceleration and deceleration intentions of humans and adjust the admittance parameter values in the same change in each direction, when the system is affected by external disturbances or noise, the robot system cannot reject or resist external disturbances, resulting in poor teaching or interaction effects.

[0004] Therefore, in robot teaching or interaction, how to adjust the robot admittance control parameters based on human intentions to improve the compliance and anti-interference ability of the robot is an important research issue.

[0005] Therefore, the technical personnel in this field are committed to developing a force guidance control method and system with variable admittance for robots. Summary of the Invention

[0006] In view of the above-mentioned defects of the prior art, the technical problem to be solved by the present invention is that in robot teaching, without presupposing a desired path and without training, it is impossible to infer the direction of human intention, and thus it is impossible to suppress the interference effects during the process.

[0007] To achieve the above object, the present invention provides a force guiding control method for variable admittance of a robot. The method obtains the interaction force between the robot and a human through a sensor, and infers the direction of the human's motion intention in real time based on the trajectory and the interaction force in the past time period, and then adjusts the inertia parameter and the damping parameter in the admittance control, so that the end of the robot complies with the motion based on the interaction force and rejects external interference. The method includes the following steps:

[0008] S101: Collect the end motion information and the human-machine interaction force information of the robot, predict the acceleration at the next moment by using the minimum jerk model and the speed curvature mode, and combine the admittance control equation with the current speed to obtain the desired force;

[0009] S103: Generate a virtual inertia and virtual damping adjustment strategy according to the direction of the desired force;

[0010] S105: Obtain the desired speed of the end of the robot and the desired angular velocity of the robot joints in the next control cycle according to the adjusted virtual inertia and virtual damping;

[0011] S107: Control the end of the robot to perform a compliant action and reject external interference.

[0012] Further, in the step S101, a six-axis force sensor is arranged at the end of the robot. The force sensor is used to detect the human-machine interaction force information and send the force sensor signal to the controller; the end motion information is obtained by each joint angle sensor of the robot, and the end motion information is obtained through the forward kinematics of the robot.

[0013] Further, calculate the speed and acceleration information of the previous N control cycles through the central difference algorithm,

[0014]

[0015]

[0016] where, V is the speed, is the acceleration, X is the position of the end of the robot, k is the kth control cycle, that is, the current control cycle, n is the nth control cycle before the current control cycle, n = 1, 2... N, N represents the number of control cycles of the speed and acceleration information applied to the above equation calculation, and T is the control cycle duration;

[0017] Calculate the speed information at the current moment through the forward difference:

[0018]

[0019] Among them, V is the speed, X is the position of the robot end, X(k) represents the current position of the robot end, X(k - 1) represents the position of the robot end in the previous control cycle, k is the kth control cycle, and T is the duration of the control cycle;

[0020] The curvature calculation model corresponding to the previous N control cycles is:

[0021]

[0022] Among them, V x represents the component of the speed on the x-axis, V y represents the component of the speed on the y-axis, represents the component of the acceleration on the x-axis, represents the component of the acceleration on the y-axis, V is the velocity vector, k is the kth control cycle, n = 1, 2…N, T is the duration of the control cycle, and κ is the calculated curvature;

[0023] The desired acceleration satisfies the speed curvature mode, and the following equation can be obtained:

[0024]

[0025] Among them, V is the velocity vector, V x represents the component of the speed on the x-axis, V y represents the component of the speed on the y-axis, is the component of the desired acceleration on the x-axis, is the component of the desired acceleration on the y-axis, k is the kth control cycle, K is the proportionality coefficient, and β is the power exponent.

[0026] Furthermore, according to the minimum jerk model of the human-planned path and the fact that the desired velocity direction is between the current force direction and the velocity direction, the desired acceleration is optimized, and the optimization model is:

[0027]

[0028]

[0029]

[0030] Among them, V is the speed, is the estimated speed of the next control cycle, is the desired acceleration, F is the human-computer interaction force vector, k is the kth control cycle, t k-1 represents the time of the previous control cycle, t k represents the time of the current control cycle;

[0031] The desired force can be derived according to the admittance control model as:

[0032]

[0033] Among them, F expected is the desired force vector, V is the velocity, is the desired acceleration, M is the default virtual inertia matrix, B is the default virtual damping matrix, k is the k-th control cycle,

[0034]

[0035] m is the default virtual inertia value, b is the default virtual damping value.

[0036] Furthermore, the step S103 further includes the following sub-steps:

[0037] S1031: Establish a coordinate system in the direction of the desired force;

[0038] S1032: According to the direction of the desired force and the direction perpendicular to the desired force, establish a transformation matrix between the coordinate system in the direction of the desired force and the base coordinate system;

[0039] S1033: Establish a strategy to suppress external disturbances and noises: when applying force in the direction of the desired force application, the admittance parameter in this direction remains the default value; when external noises or disturbances are introduced, the admittance parameter in the direction perpendicular to the desired direction will increase, making it more difficult to move in the direction perpendicular to the desired direction;

[0040] Among them, the transformation matrix is:

[0041]

[0042]

[0043] Among them, M d is the virtual inertia matrix, B d is the virtual damping matrix, R is the transformation matrix between the coordinate system in the direction of the desired force and the base coordinate system, γ is the proportionality coefficient, γ = 1 + 10||V(k)||2, V is the velocity, k is the k-th control cycle, m is the default virtual inertia value, b is the default virtual damping value.

[0044] Furthermore, in the step S105, the admittance control equation of the robot is:

[0045]

[0046] Among them, M d is the virtual inertia matrix, B d is the virtual damping matrix, X is the position in the Cartesian coordinate system, F is the human-robot interaction force;

[0047] Based on the virtual inertia matrix, the virtual damping matrix, the human - machine interaction force, and the velocity at the current moment, the desired acceleration of the robot end - effector in the next control cycle can be calculated as follows:

[0048]

[0049] where is the desired end - effector velocity value sent to the robot, is the end - effector velocity of the robot in the previous control cycle, F(k) is the human - machine interaction force, M d is the virtual inertia matrix, B d is the virtual damping matrix, k represents the k - th control cycle, and T represents the control cycle duration;

[0050] The desired angular velocity of the robot joints can be obtained by the inverse Jacobian matrix:

[0051]

[0052] where is the desired joint angular velocity, is the inverse Jacobian matrix, is the desired end - effector velocity value sent to the robot, and k represents the k - th control cycle.

[0053] On the other hand, the present invention also provides a force - guiding control system with variable admittance for a robot. The control system adopts the force - guiding control method with variable admittance for a robot provided by the present invention, and includes a human - machine interaction module, a human intention prediction module, a variable - admittance control module, and a robot end - effector position control module. Among them,

[0054] The human - machine interaction module includes the robot end - effector;

[0055] The human intention prediction module collects the end - effector motion information and the human - machine interaction force information of the robot, applies the minimum - jerk model and the velocity curvature pattern to predict the acceleration at the next moment, and combines the admittance control equation and the current velocity to obtain the desired force;

[0056] The variable - admittance control module, according to the direction of the desired force, establishes the current desired - force coordinate system and generates the virtual inertia and virtual damping adjustment strategies;

[0057] The robot end - effector position control module, according to the adjusted virtual inertia and virtual damping, obtains the desired velocity of the robot end - effector in the next control cycle, further obtains the joint angular velocity of the robot, and controls the robot end - effector to perform compliant actions.

[0058] Further, the human intention prediction module uses the following calculation model to calculate the desired force:

[0059]

[0060] where F expected is the desired force vector, M is the default virtual inertia matrix, B is the default virtual damping matrix, V is the current velocity vector, is the desired acceleration, and k represents the k-th control cycle.

[0061] Further, the admittance control equation used by the variable admittance control module is:

[0062]

[0063] where M d is the virtual inertia matrix, B d is the virtual damping matrix, X is the position in the Cartesian coordinate system, and F is the human-robot interaction force between the robot and the human;

[0064] Further, the robot end position control module uses the following method to calculate the desired velocity of the robot end for the next control cycle:

[0065]

[0066] where is the desired end velocity value sent to the robot in the current control cycle, is the desired end velocity value sent to the robot in the previous control cycle, is the robot end velocity in the previous control cycle, F(k) is the human-robot interaction force in the current control cycle, M d is the virtual inertia matrix, B d is the virtual damping matrix, k represents the k-th control cycle, and T represents the control cycle duration.

[0067] In the preferred embodiment of the present invention, compared with the prior art, the present invention has the following beneficial effects:

[0068] 1. The present invention predicts the desired acceleration through the minimum jerk model of the human planned path and the velocity curvature power law spectrum, so as to predict the direction of the force and perform force guidance. The present invention does not require preset trajectories and preconditions, improves the accuracy, smoothness and intuitiveness of the taught trajectory, and lays a foundation for the subsequent reproduction of the trajectory;

[0069] 2. The present invention improves the applicability of the human-robot interaction process and can be more widely applied to a variety of unknown scenarios.

[0070] The concept, specific structure and technical effects of the present invention will be further described below in conjunction with the accompanying drawings to fully understand the purpose, features and effects of the present invention. Description of the Drawings

[0071] Figure 1 is a flowchart of a force guidance control method for variable admittance of a robot according to a preferred embodiment of the present invention;

[0072] Figure 2 is a control block diagram of a force guidance control system for variable admittance of a robot according to a preferred embodiment of the present invention;

[0073] Figure 3 is a schematic diagram of a force guidance control method for variable admittance of a robot according to a preferred embodiment of the present invention. Detailed Embodiments

[0074] The following introduces multiple preferred embodiments of the present invention with reference to the accompanying drawings of the specification to make its technical content clearer and easier to understand. The present invention can be embodied in many different forms of embodiments, and the protection scope of the present invention is not limited to the embodiments mentioned in the text.

[0075] In the accompanying drawings, components with the same structure are denoted by the same reference numerals, and components with similar structures or functions are denoted by similar reference numerals. The size and thickness of each component shown in the drawings are arbitrarily shown, and the present invention does not limit the size and thickness of each component. In order to make the illustration clearer, the thickness of some components in the drawings is appropriately exaggerated.

[0076] In the existing robot teaching, without presetting the desired path and without training, it is impossible to infer the direction of human intention, and thus it is impossible to suppress the interference influence during the process. The present invention predicts the desired acceleration through the minimum jerk model of the human planned path verified theoretically and the observed velocity curvature power law spectrum phenomenon, so as to be able to predict the direction of force and conduct force guidance.

[0077] As Figure 1 shown, a force guidance control method for variable admittance of a robot provided by an embodiment of the present invention obtains the interaction force between the robot and a human through a sensor, and infers the direction of the human motion intention in real time according to the trajectory and the human-robot interaction force in the past time period, and then adjusts the inertia parameter and the damping parameter in the admittance control, so that the robot end complies with the motion based on the human-robot interaction force and rejects external interference.

[0078] Specifically, the method includes the following steps:

[0079] S101: Collect the end motion information and human-machine interaction force information of the robot, predict the acceleration at the next moment using the minimum jerk model and speed curvature mode, and combine the admittance control equation with the current speed to obtain the desired force.

[0080] In the above steps, a six-axis force sensor is set at the end of the robot. This force sensor is used to detect the human-machine interaction force information and send the force sensor signal to the controller; the end motion information of the robot is obtained by each joint angle sensor of the robot, and the end motion information is obtained through the forward kinematics of the robot.

[0081] Calculate the speed and acceleration information of the previous N control cycles through the central difference algorithm.

[0082]

[0083]

[0084] Among them, V is the speed. is the acceleration, X is the position of the robot end, k is the kth control cycle, that is, the current control cycle, n is the nth control cycle before the current control cycle, n = 1, 2... N, N represents the number of control cycles of the speed and acceleration information applied to the above equation calculation, and T is the control cycle duration.

[0085] Calculate the speed information at the current moment through forward difference:

[0086]

[0087] Among them, V is the speed, X is the position of the robot end, X(k) represents the current position of the robot end, X(k - 1) represents the position of the robot end in the previous 1 control cycle, k is the kth control cycle, and T is the control cycle duration.

[0088] The curvature calculation model corresponding to the previous N control cycles is:

[0089]

[0090] Among them, V x represents the component of the speed in the x-axis, V y represents the component of the speed in the y-axis. represents the component of the acceleration in the x-axis. represents the component of the acceleration in the y-axis, V is the speed vector, k is the kth control cycle, n = 1, 2... N, T is the control cycle duration, and κ is the calculated curvature.

[0091] The desired acceleration satisfies the speed curvature mode, and the following equation can be obtained:

[0092]

[0093] Wherein, V is the velocity vector, V x represents the component of the velocity on the x-axis, V y represents the component of the velocity on the y-axis, is the component of the desired acceleration on the x-axis, is the component of the desired acceleration on the y-axis, k is the k-th control period, K is the proportionality coefficient, and β is the power exponent.

[0094] According to the minimum jerk model of the human planned path, and the desired velocity direction is between the current force direction and the velocity direction, the desired acceleration is optimized, and the optimization model is:

[0095]

[0096]

[0097]

[0098] Wherein, V is the velocity, is the estimated velocity for the next control period, is the desired acceleration, F is the human-machine interaction force vector, k is the k-th control period, t k-1 represents the time of the previous control period, t k represents the time of the current control period;

[0099] The desired force can be derived according to the admittance control model as:

[0100]

[0101] Wherein, F expected is the desired force vector, V is the velocity, is the desired acceleration, M is the default virtual inertia matrix, B is the default virtual damping matrix, k is the k-th control period,

[0102]

[0103] m is the default virtual inertia value, and b is the default virtual damping value.

[0104] S103: Generate a virtual inertia and virtual damping adjustment strategy according to the direction of the desired force.

[0105] The above steps include the following sub-steps:

[0106] S1031: Establish a coordinate system for the direction of the desired force;

[0107] S1032: Establish the transformation matrix between the coordinate system of the desired force direction and the base coordinate system according to the desired force direction and the direction perpendicular to the desired force direction;

[0108] S1033: Establish a strategy to suppress external disturbances and noise: when applying force in the desired force application direction, the admittance parameter in this direction remains the default value; when external noise or disturbance is introduced, the admittance parameter in the direction perpendicular to the desired direction will increase, making it more difficult to move in the direction perpendicular to the desired direction;

[0109] Among them, the above transformation matrix is:

[0110]

[0111]

[0112] Among them, M d is the virtual inertia matrix, B d is the virtual damping matrix, R is the transformation matrix between the coordinate system of the desired force direction and the base coordinate system, γ is the proportionality coefficient, γ = 1 + 10||V(k)||2, V is the velocity, k is the kth control cycle, m is the default virtual inertia value, and b is the default virtual damping value.

[0113] S105: Obtain the desired velocity of the robot end and the desired angular velocity of the robot joints in the next control cycle according to the adjusted virtual inertia and virtual damping.

[0114] In the above steps, the admittance control equation of the robot is:

[0115]

[0116] Among them, M d is the virtual inertia matrix, B d is the virtual damping matrix, X is the position in the Cartesian coordinate system, and F is the human-machine interaction force;

[0117] According to the virtual inertia matrix, virtual damping matrix, human-machine interaction force, and velocity at the current moment, the desired acceleration of the robot end in the next control cycle can be calculated:

[0118]

[0119] Among them, is the desired end velocity value sent to the robot, is the robot end velocity in the previous control cycle, F(k) is the human-machine interaction force, M d is the virtual inertia matrix, B d is the virtual damping matrix, k represents the kth control cycle, and T represents the control cycle duration;

[0120] The desired angular velocity of the robot joint can be obtained from the inverse Jacobian matrix:

[0121]

[0122] where, is the desired joint angular velocity, is the inverse Jacobian matrix, is the desired end - effector velocity value sent to the robot, and k represents the k - th control cycle.

[0123] S107: Control the end - effector of the robot to perform a compliant action and reject external interference.

[0124] A force - guiding control method with variable admittance for a robot provided by an embodiment of the present invention, compared with a control method that does not consider the direction of human intention, does not require a preset trajectory and pre - conditions, improves the accuracy, smoothness and intuitiveness of the taught trajectory, and lays a foundation for subsequent trajectory reproduction; compared with existing patents, it improves the applicability of the human - robot interaction process and can be more widely applied to various unknown scenarios.

[0125] In addition, the present invention utilizes the principle of coordinate system transformation to transform the admittance parameter matrix in real - time, making the admittance parameter in the desired force direction low and the admittance parameter perpendicular to the desired force direction high. Compared with the independent - of - all - directions admittance control, the low admittance in the desired force direction makes the robot more compliant, and the high admittance perpendicular to the desired force direction makes the robot resist the force in this direction, constituting a force - guiding effect, suppressing the interference of the external environment, and improving the accuracy and smoothness of the taught trajectory.

[0126] An embodiment of the present invention also provides a force - guiding control system with variable admittance for a robot. This control system adopts the force - guiding control method with variable admittance for a robot provided by the present invention, and includes a human - robot interaction module, a human intention prediction module, a variable - admittance control module, and a robot end - effector position control module. Among them,

[0127] The human - robot interaction module includes the robot end - effector;

[0128] The human intention prediction module collects the end - effector motion information and human - robot interaction force information of the robot, predicts the acceleration at the next moment by applying the minimum - jerk model and the velocity curvature pattern, and combines the admittance control equation and the current velocity to obtain the desired force;

[0129] The variable - admittance control module sets up the current desired force coordinate system according to the direction of the desired force and generates a virtual inertia and virtual damping adjustment strategy;

[0130] The robot end - position control module, based on the adjusted virtual inertia and virtual damping, obtains the expected velocity of the robot end for the next control cycle, further obtains the joint angular velocity of the robot, and controls the robot end to perform compliant actions.

[0131] The human intention prediction module uses the following calculation model to calculate the expected force:

[0132]

[0133] Where, F expected is the expected force vector, M is the default virtual inertia matrix, B is the default virtual damping matrix, V is the current velocity vector, is the expected acceleration, and k represents the k - th control cycle.

[0134] The admittance control equation used by the variable - admittance control module is:

[0135]

[0136] Where, M d is the virtual inertia matrix, B d is the virtual damping matrix, X is the position in the Cartesian coordinate system, and F is the human - robot interaction force between the robot and the human;

[0137] The robot end - position control module uses the following method to calculate the expected velocity of the robot end for the next control cycle:

[0138]

[0139] Where, is the expected end - velocity value sent to the robot in the current control cycle, is the expected end - velocity value sent to the robot in the previous control cycle, is the robot end - velocity in the previous control cycle, F(k) is the human - robot interaction force in the current control cycle, M d is the virtual inertia matrix, B d is the virtual damping matrix, k represents the k - th control cycle, and T represents the control - cycle duration.

[0140] The present invention will be described in detail below in conjunction with the preferred embodiments of the present invention.

[0141] As Figure 1 、 Figure 2 and Figure 3As shown in the figure, a force guidance control method with variable admittance for a robot provided by a preferred embodiment of the present invention requires the end of the robot to follow the human's movement compliantly and resist external interference during the human-robot collaborative interaction process. The present invention obtains the interaction force between the robot and the human through sensors, and infers the direction of the human's movement intention in real time based on the trajectory and interaction force in the past time period, and then adjusts the inertia parameter and damping parameter in the admittance control, so that the end of the robot can comply with the movement based on the interaction force of the human, and at the same time provide a force guidance effect to the human to reject external interference.

[0142] As Figure 1 shown in the figure, a force guidance control method with variable admittance for a robot provided by a preferred embodiment of the present invention includes the following specific steps:

[0143] Step 1: Collect the motion information of the robot end and the human-robot interaction force information, predict the acceleration at the next moment using the minimum jerk model and the speed curvature mode, and combine the admittance control equation with the current speed to obtain the desired force.

[0144] Specifically, during the direct teaching process, the human pulls the end of the robot to move. A six-axis force sensor is fixed at the end of the robot. The force sensor is used to detect the interaction force between the human and the robot during the process of the human pulling the robot, and send the force sensor signal to the controller. The end information of the robot is obtained by the joint angle sensors of the robot, and the motion information of the robot end is obtained through the forward kinematics of the robot. The speed and acceleration information of the previous N control cycles are obtained through the central difference algorithm:

[0145]

[0146]

[0147] where n = 1, 2... N, n represents that the calculated speed and acceleration information are for the nth control cycle before the current control cycle, and N represents the number of control cycles of the speed and acceleration information applied to the above equation calculation represents acceleration, V represents speed, k represents the current kth control cycle, that is, the current control cycle, X represents the position of the robot end, and T represents the control cycle duration.

[0148] The speed information at the current moment is obtained through forward difference:

[0149]

[0150] where V(k) represents the calculated speed of the current control cycle, X(k) represents the current position of the robot end, X(k - 1) represents the position of the robot end in the previous 1 control cycle, and T represents the control cycle duration.

[0151] The calculation model for the curvature corresponding to the first N control cycles is as follows:

[0152]

[0153] Among them, V x represents the component of the velocity on the x-axis. Similarly, V y represents the component of the velocity on the y-axis. represents the component of the acceleration on the x-axis. represents the component of the acceleration on the y-axis. V represents the velocity vector, and κ is the calculated curvature.

[0154] In the velocity curvature mode, the proportionality coefficient and the power exponent are obtained by the least squares method. Taking κ(k - n), n = 1, 2... N as the input and V(k - n), n = 1, 2... N as the output, the proportionality coefficient K and the power exponent β are identified in real time.

[0155] The desired acceleration should also satisfy the velocity curvature mode, and the following equation can be obtained:

[0156]

[0157] Among them, is the component of the desired acceleration on the x-axis. is the component of the desired acceleration on the y-axis. K is the proportionality coefficient, β is the power exponent, V(k) represents the velocity of the current control cycle obtained by calculation, V x represents the component of the velocity on the x-axis. Similarly, V y represents the component of the velocity on the y-axis.

[0158] According to the minimum jerk model of the human planned path, and at the same time satisfying that the desired velocity direction should be between the current force direction and the velocity direction, the desired acceleration is optimized, and the model is:

[0159]

[0160]

[0161]

[0162] Among them, represents the prediction of the velocity of the next control cycle relative to the current control cycle. represents the third derivative of the position in the x direction with respect to time t. represents the third derivative of the position in the y direction with respect to time t, t k-1 represents the time of the previous control cycle, t k represents the time of the current control cycle. F(k) represents the interaction force vector read by the sensor in the current control cycle.

[0163] The desired force can be derived according to the admittance control model as follows:

[0164]

[0165]

[0166]

[0167] where F expected is the desired force vector, M is the default virtual inertia matrix, which is a diagonal matrix with equal elements on the diagonal, m is the default virtual inertia value, B is the default virtual damping matrix, which is a diagonal matrix with equal elements on the diagonal, b is the default virtual damping value, represents the desired acceleration vector in the current control period, and V(k) represents the velocity value in the current period calculated by forward difference.

[0168] Step 2: According to the direction of the desired force, different from the tool coordinate system and the base coordinate system, establish the current desired force coordinate system, and generate the virtual inertia and virtual damping adjustment strategies.

[0169] Specifically, the method of the present invention first establishes a coordinate system in the direction of the desired force. By constructing the direction of the desired force and the direction perpendicular to the desired force, the transformation matrix R between the constructed coordinate system and the base coordinate system can be obtained. The strategy for suppressing external disturbances and noises in the present invention is: when a person applies force in the direction where the person expects to apply force, the admittance parameters in this direction remain the default values; when external noises or disturbances are introduced, the admittance parameters perpendicular to the direction expected by the person will become larger, making it more difficult to move perpendicular to the expected direction.

[0170]

[0171]

[0172] where R represents the transformation matrix between the coordinate system with the direction of the desired force as the x-axis and the direction perpendicular to the desired force as the y-axis and the base coordinate system of the robot, M d is the virtual inertia matrix, B d is the virtual damping matrix, m is the default virtual inertia value, b is the default virtual damping value, γ is the proportionality coefficient, and the increase in the admittance parameter perpendicular to the direction of the desired force is proportional to the current velocity:

[0173] γ = 1 + 10||V(k)||2

[0174] where γ is the proportionality coefficient and V(k) is the velocity in the current period obtained by forward difference calculation.

[0175] Such as Figure 2As shown, the interaction force between the robot and the human is obtained through sensors, and the human's motion intention direction is inferred in real time based on the trajectory and interaction force in the past time period. Then, the inertial parameters and damping parameters in the admittance control are adjusted, and the adjusted M d virtual inertial matrix and B d virtual damping matrix are transmitted to the admittance control, so that while the robot end can comply with the movement based on the human's interaction force, it can provide a force guiding effect to the human to reject external disturbances. Figure 2 The dashed line in Figure 2 represents the two parameters m and b whose admittance parameters are modulated and changed, and are also transmitted into the admittance control. This dashed line does not point, but indicates passing through the admittance control, realizing the update of the parameters.

[0176] Step 3: According to the adjusted virtual inertia and virtual damping, obtain the desired velocity of the robot end in the next control cycle, and further obtain the corresponding desired robot joint angular velocity, so as to control the robot end to perform compliant actions under the force guiding control method of variable admittance of the robot based on human intention recognition.

[0177] Specifically, the admittance control model of the robot is:

[0178]

[0179] where M d is the virtual inertial matrix, B d is the virtual damping matrix, X is the position in the Cartesian coordinate system, and F is the interaction force between the robot and the human.

[0180] According to the virtual inertial matrix, virtual damping matrix, interaction force, and velocity at the current moment, the desired acceleration of the robot end in the next control cycle can be calculated as:

[0181]

[0182] where k represents the kth control cycle, T represents the control cycle duration, represents the desired end velocity value sent to the robot in the current control cycle, represents the desired end velocity value sent to the robot in the previous control cycle, F(k) represents the interaction force read by the force sensor in the current control cycle, represents the robot end velocity in the previous control cycle obtained through calculation.

[0183] The desired robot joint angular velocity can be obtained by the inverse Jacobian matrix:

[0184]

[0185] where is the desired joint angular velocity, is the inverse Jacobian matrix, representing the desired end - effector velocity value sent to the robot in the current control cycle.

[0186] In a preferred embodiment of the present invention, the present invention further provides a force - guiding control system with variable admittance for a robot, including a human - machine interaction module, a human intention prediction module, a variable - admittance control module, and a robot end - effector position control module. Among them,

[0187] the human - machine interaction module includes a human and the robot end - effector, and the human pulls the robot end - effector;

[0188] the human intention prediction module collects the robot end - effector motion information and the human - machine interaction force information, predicts the acceleration at the next moment using the minimum - jerk model and the velocity - curvature pattern, and combines the admittance control equation and the current velocity to obtain the desired force;

[0189] Further preferably, the calculation model of the desired force is:

[0190]

[0191]

[0192]

[0193] where, F expected is the desired force vector, M is the default virtual inertia matrix, which is a diagonal matrix and the elements on the diagonal are equal, m is the default virtual inertia value, B is the default virtual damping matrix, which is a diagonal matrix and the elements on the diagonal are equal, b is the default virtual damping value, represents the desired acceleration vector in the current control cycle, V(k) represents the velocity value of the current cycle calculated by forward difference, and k is the k - th control cycle.

[0194] The variable - admittance control module sets up the current desired - force coordinate system according to the direction of the desired force, different from the tool coordinate system and the base coordinate system, and generates the virtual inertia and virtual damping adjustment strategies;

[0195] As a further preference, the robot variable - admittance control model is:

[0196]

[0197] where, M d is the virtual inertia matrix, B d is the virtual damping matrix, X is the position in the Cartesian coordinate system, and F is the interaction force between the robot and the human.

[0198] The robot end - position control module obtains the expected velocity of the robot end in the next control period based on the adjusted virtual inertia and virtual damping, and further obtains the corresponding expected angular velocity of the robot joints, so as to control the robot end to perform compliant actions under the force - guiding control method of robot variable admittance based on human intention recognition.

[0199] As a further preference, the expected velocity is:

[0200]

[0201] where \(k\) represents the \(k\) - th control period, \(T\) represents the control - period duration, represents the expected end - velocity value sent to the robot in the current control period, represents the expected end - velocity value sent to the robot in the previous control period, \(F(k)\) represents the interaction force read by the force sensor in the current control period,

[0202] represents the robot end - velocity in the previous control period obtained by calculation.

[0203] In the robot teaching of the prior art, without presetting the expected path and without training, it is impossible to infer the direction of human intention, and thus impossible to suppress the interference influence during the process. The preferred embodiment of the present invention predicts the expected acceleration through the minimum - jerk model of the human - planned path verified theoretically and the observed speed - curvature power - law spectrum phenomenon, so as to be able to predict the direction of force and perform force guidance. Compared with the control methods in the prior art that do not consider the direction of human intention, the present invention does not require presetting trajectories and pre - conditions, improves the accuracy, smoothness and intuitiveness of the teaching trajectory, lays a foundation for the subsequent reproduction of the trajectory, improves the applicability of the human - machine interaction process, and can be more widely applied to a variety of unknown scenarios.

[0204] In addition, the admittance parameters in the current variable - admittance control are mostly diagonal matrices and isotropic, unable to comply with the real - time changing expected direction and resist the undesired direction. The preferred embodiment of the present invention solves the problem of the expected force direction, uses the coordinate - transformation principle to transform the admittance - parameter matrix in real time, so that the admittance parameter in the expected - force direction is low and the admittance parameter perpendicular to the expected - force direction is high. Compared with the isotropic admittance control, the low admittance in the expected - force direction makes the robot more compliant, and the high admittance perpendicular to the expected - force direction makes the robot resist the force in this direction, forming a force - guiding effect, suppressing the interference of the external environment, and improving the accuracy and smoothness of the teaching trajectory.

[0205] Therefore, compared with the prior art, the present invention can achieve better technical effects. By obtaining the interaction force between the robot and the human and the trajectory in the past time period, the intention direction of the human is inferred in real time without presetting the trajectory and preconditions, improving the applicability and intuitiveness of the human-computer interaction process and being more widely applied to various unknown scenarios. The variable admittance force guidance method based on human intention recognition can improve the compliance of the robot, provide force guidance at the same time, suppress the interference of the external environment, and improve the accuracy and smoothness of compliance.

[0206] The preferred specific embodiments of the present invention have been described in detail above. It should be understood that those of ordinary skill in the art can make many modifications and variations according to the concept of the present invention without creative labor. Therefore, all technical solutions that can be obtained by those skilled in the art in the technical field based on the concept of the present invention through logical analysis, reasoning or limited experiments on the basis of the prior art should fall within the protection scope determined by the claims.

Claims

1. A force guidance control method for variable admittance of a robot, characterized in that, The method obtains the interaction force between the robot and the human through a sensor, and infers the direction of the human's motion intention in real time based on the trajectory and the interaction force in the past time period, and then adjusts the inertia parameter and the damping parameter in the admittance control, so that the end of the robot complies with the motion based on the interaction force and rejects external interference. The method includes the following steps: S101: Collect the end motion information and the human-machine interaction force information of the robot, predict the acceleration at the next moment by using the minimum jerk model and the velocity curvature mode, and combine the admittance control equation and the current velocity to obtain the desired force; S103: Generate a virtual inertia and virtual damping adjustment strategy according to the direction of the desired force; S105: Obtain the desired velocity of the end of the robot and the desired angular velocity of the robot joints in the next control cycle according to the adjusted virtual inertia and virtual damping; S107: Control the end of the robot to perform a compliant action and reject external interference.

2. The control method according to claim 1, wherein In the step S101, a six-axis force sensor is set at the end of the robot. The force sensor is used to detect the human-machine interaction force information and send the force sensor signal to the controller; the end motion information is obtained by the respective joint angle sensors of the robot, and the end motion information is obtained through the forward kinematics of the robot.

3. The control method according to claim 2, characterized in that, Calculate the velocity and acceleration information of the previous N control cycles through the central difference algorithm, where V is the velocity, is the acceleration, X is the position of the robot end-effector, k is the k-th control period, i.e., the current control period, n is the n-th control period before the current control period, n = 1, 2... N, N represents the number of control periods for the velocity and acceleration information used in the above equations, and T is the control period duration; Calculate the velocity information at the current moment through the forward difference: Where, V is the velocity, X is the position of the end of the robot, X(k) represents the current position of the end of the robot, X(k - 1) represents the position of the end of the robot in the previous 1 control cycle, k is the kth control cycle, and T is the duration of the control cycle; The curvature calculation model corresponding to the previous N control cycles is: Among them, V x represents the component of the velocity on the x-axis, V y represents the component of the velocity on the y-axis, represents the component of the acceleration on the x-axis, represents the component of the acceleration on the y-axis, V is the velocity vector, k is the k-th control period, n = 1, 2…N, T is the duration of the control period, and κ is the calculated curvature; The desired acceleration satisfies the velocity curvature mode, and the following equation can be obtained: Among them, V is the velocity vector, V x represents the component of the velocity on the x-axis, V y represents the component of the velocity on the y-axis, is the component of the desired acceleration on the x-axis, is the component of the desired acceleration on the y-axis, k is the k-th control period, K is the proportionality coefficient, and β is the power exponent.

4. The control method according to claim 3, characterized in that According to the minimum jerk model of the human planned path, and the desired velocity direction is between the current force direction and the velocity direction, optimize the desired acceleration, and the optimization model is: where V is the speed, is the predicted speed for the next control cycle, is the desired acceleration, F is the human-machine interaction force vector, k is the k-th control cycle, t k-1 represents the time of the previous control cycle, t k represents the time of the current control cycle; The desired force can be derived according to the admittance control model as: Among them, F expected is the desired force vector, V is the velocity, is the desired acceleration, M is the default virtual inertia matrix, B is the default virtual damping matrix, and k is the k-th control cycle, m is the default virtual inertia value, and b is the default virtual damping value.

5. The control method according to claim 1, wherein The step S103 further includes the following sub-steps: S1031: Establish a coordinate system in the direction of the desired force; S1032: Establish a transformation matrix between the coordinate system in the direction of the desired force and the base coordinate system according to the direction of the desired force and the direction perpendicular to the desired force; S1033: Establish a strategy to suppress external disturbances and noises: when a force is applied in the direction of the desired force, the admittance parameter in this direction remains the default value; when external noises or disturbances are introduced, the admittance parameter in the direction perpendicular to the desired direction will become larger, making it more difficult to move in the direction perpendicular to the desired direction; Where, the transformation matrix is: Among them, M d is the virtual inertia matrix, B d is the virtual damping matrix, R is the transformation matrix between the desired force direction coordinate system and the base coordinate system, γ is the proportionality coefficient, γ = 1 + 10||V(k)||2, V is the velocity, k is the k-th control period, m is the default virtual inertia value, and b is the default virtual damping value.

6. The control method according to claim 1, wherein In the step S105, the admittance control equation of the robot is: where M d is the virtual inertia matrix, B d is the virtual damping matrix, X is the position in the Cartesian coordinate system, and F is the human-computer interaction force; According to the virtual inertia matrix, the virtual damping matrix, the human-machine interaction force and the velocity at the current moment, the desired acceleration of the end of the robot in the next control cycle can be calculated: Among them, is the desired end velocity value sent to the robot, is the end velocity of the robot in the previous control cycle, F(k) is the human-robot interaction force, M d is the virtual inertia matrix, B d is the virtual damping matrix, k represents the k-th control cycle, and T represents the control cycle duration; The desired angular velocity of the robot joints can be obtained by the inverse Jacobian matrix: wherein, is the desired joint angular velocity, is the inverse Jacobian matrix, is the desired end-effector velocity value sent to the robot, and k represents the k-th control cycle.

7. A force guidance control system with variable admittance for a robot, characterized in that, The control system adopts the control method described in any one of claims 1-6, and includes a human-computer interaction module, a human intention prediction module, a variable admittance control module, and a robot end position control module, wherein, the human-computer interaction module includes the robot end; the human intention prediction module collects the end motion information and the human-computer interaction force information of the robot, applies the minimum jerk model and the velocity curvature pattern to predict the acceleration at the next moment, and combines the admittance control equation and the current velocity to obtain the desired force; the variable admittance control module establishes the current desired force coordinate system according to the direction of the desired force, and generates the virtual inertia and the virtual damping adjustment strategy; the robot end position control module obtains the desired velocity of the robot end at the next control cycle according to the adjusted virtual inertia and virtual damping, further obtains the joint angular velocity of the robot, and controls the robot end to perform a compliant action.

8. The control system according to claim 7, wherein The human intention prediction module adopts the following calculation model to calculate the desired force: where, F expected is the desired force vector, M is the default virtual inertia matrix, B is the default virtual damping matrix, V is the current velocity vector, is the desired acceleration, and k represents the k-th control cycle.

9. The control system according to claim 7, characterized in that, The admittance control equation used by the variable admittance control module is: Among them, M d is the virtual inertia matrix, B d is the virtual damping matrix, X is the position in the Cartesian coordinate system, and F is the human-robot interaction force between the robot and the human.

10. The control system according to claim 7, wherein The robot end position control module uses the following method to calculate the desired velocity of the robot end at the next control cycle: Among them, is the desired end - effector velocity value sent to the robot in the current control cycle, is the desired end - effector velocity value sent to the robot in the previous control cycle, is the end - effector velocity of the robot in the previous control cycle, F(k) is the human - robot interaction force in the current control cycle, M d is the virtual inertia matrix, B d is the virtual damping matrix, k represents the k - th control cycle, and T represents the control cycle duration.

Images