CN111037560B - A collaborative robot compliance force control method and system - Google Patents

Abstract

The invention discloses a compliance force control method and system for a collaborative robot, wherein the method includes: obtaining a state variable of a robot and initializing it; obtaining a current rotation matrix based on the initialization state variable; reading the robot's state variable based on the current rotation matrix Current state feedback information; based on the current state feedback information, construct the equality constraints to realize compliance force control and the inequality constraints of joint angle, joint angular velocity and joint torque in the robot system; rewrite the joint torque function and obtain the final constraint optimization model ; Update the state variables and control torque in the final constrained optimization model based on the dynamic neural network model; judge whether the current time is greater than the task time, if so, end the compliance force control, otherwise, return to obtain the current rotation matrix. In the embodiment of the present invention, high-precision force control in the direction of the contact force and motion control in the direction of free movement can be simultaneously achieved.

Description

A collaborative robot compliance force control method and system

technical field

The invention relates to the technical field of intelligent control of robots, in particular to a method and system for controlling the compliance force of a collaborative robot.

Background technique

A collaborative robot is a robot that can work together with humans in a co-working space. Collaborative robots can directly work side by side with human employees without using safety fences for isolation, and have the characteristics of rapid production line deployment, simple task switching, and good human-machine friendliness. It has broad application prospects in other fields, and is considered to be an important carrier for the realization of "Industry 4.0" and "Intelligent Manufacturing 2025".

The most important prerequisite for collaborative robots to enter practical applications is to achieve "human-machine integration", in which it is particularly important to improve the flexibility of the system. Force control can improve the flexibility of the system, enhance human-robot interaction, provide intelligent responses, and enable robots to operate autonomously in weakly structured and unstructured environments. Therefore, its application fields are more extensive, such as flexible assembly, coordinated operation of two arms, human-computer interaction, grasping objects with dexterous hands, and gait control of footed robots. The research on redundant robot force control has strong application value and practical significance.

The existing force control methods for collaborative robots with redundant degrees of freedom are mainly based on the pseudo-inverse calculation of the Jacobian matrix of the robot; however, this method generally has the following problems: 1) The pseudo-inverse calculation of the Jacobian matrix leads to the calculation of Excessive cost; 2) Difficult to deal with the physical constraints of the system.

SUMMARY OF THE INVENTION

The purpose of the present invention is to overcome the deficiencies of the prior art. The present invention provides a method and system for controlling the compliance force of a collaborative robot, which can simultaneously realize high-precision force control in the direction of contact force and motion control in the direction of free movement, and can realize the The online optimization of joint torque can ensure that the robot does not exceed its physical constraints during the compliance force control process.

In order to solve the above technical problems, an embodiment of the present invention provides a method for controlling the compliance force of a collaborative robot, the method comprising:

Get the state variables of the robot and initialize them;

Obtain the current rotation matrix based on the initialized state variable;

Read the current state feedback information of the robot based on the current rotation matrix;

Constructing equality constraints to realize compliance force control and inequality constraints of joint angles, joint angular velocities and joint moments in the robot system based on the current state feedback information;

Rewrite the joint moment function and obtain the final constraint optimization model;

Update the state variables and control torques in the final constrained optimization model based on the dynamic neural network model;

Determine whether the current time is greater than the task time, and if so, end the compliance control of the robot, otherwise, return to obtain the current rotation matrix based on the initialization state variable.

Optionally, before obtaining and initializing the state variables of the robot, the method further includes:

Aiming at the orthogonal characteristics of the contact force between the robot motion and the workpiece, modeling is carried out in the tool coordinate system and the base coordinate system respectively to obtain a robot motion modeling system;

Among them, the base coordinate system represents R ₀ (x ₀ , y ₀ , z ₀ ); the tool coordinate system represents R _t (x _t , y _t , z _t ).

Optionally, the contact force between the robot and the workpiece is parallel to z _t in the tool coordinate system, while x _t and y _t define the free movement of the robot end effector;

During the operation of the robot, there is a slight deviation between the actual position _x of the _robot end _effector and the expected trajectory _{x d} . The coordinate system R _t (x _t , y _t , z _t ) is described as the deviation δX of the robot from the desired trajectory under the base frame and the deviation δX of the robot from the desired trajectory under the tool coordinate system, respectively _t .

Optionally, separately modeling is performed in the tool coordinate system and the base coordinate system, and the process of obtaining the robot motion modeling system is as follows:

In the tool coordinate system R _t (x _t , y _t , z _t ), since the friction between the robot and the workpiece is ignored, assuming that the contact between the robot and the workpiece is a rigid contact, then The contact force can be described as:

F _t =k _f ∑ _t δX _t ; (1)

Among them, k _f represents the stiffness coefficient; ∑ _t =diag(0,0,1) is a diagonal matrix, which is used to describe the deviation δX _t of the robot from the desired trajectory in the tool coordinate system and its contact force. relationship, where 0 means that the displacement in this direction will not generate contact force, otherwise, take 1;

则在所述工具坐标系R_t(x_t,y_t,z_t)下的位置跟踪误差e_t为：definition

Then the position tracking error e _t under the tool coordinate system R _t (x _t , y _t , z _t ) is:

When the contact surface is known, a rotation matrix S _t is used to describe the tool coordinate system R _t (x _t , y _t , z _t ) and the base frame R ₀ (x ₀ , y ₀ , z ₀ ) The rotation relationship between;

Define F and e ₀ as the corresponding descriptions of δX _t and F _t under the base frame R ₀ (x ₀ , y ₀ , z ₀ ), respectively, then there are:

e _t =S _t e ₀ ; (4)

δX _t =S _t δX; (5)

Through the combination of formulas (1)-(5), we have:

Wherein, δX represents the deviation of the robot from the expected trajectory under the base frame R ₀ (x ₀ , y ₀ , z ₀ ); F represents the base frame R ₀ (x ₀ , y ₀ , contact force at z ₀ );

The displacement δX in the base frame R ₀ (x ₀ , y ₀ , z ₀ ) can be described as δX=xx _d , where the desired trajectory x _d is the desired position signal described in R ₀ , therefore, for formula (6 )-(7) is rewritten as:

表示描述运动的参数矩阵。Define the desired trajectory and command force of the robot as x _d and F _d respectively; then according to the description of formula (5) and formula (9), the control objective can be described as designing a position-oriented control strategy for redundant robots, so that Contact force F→F _d described by formula (8), while making the tracking error e ₀ described by formula (9) → 0; ∑ _t represents the parameter matrix describing the contact;

Represents a matrix of parameters describing motion.

r_d＝[F_d；0]，

则有公式(8)和公式(9)重写为：Optionally, in the robot motion modeling system, in order to simplify the description, define

r _d =[F _d ; 0],

Then there are formulas (8) and (9) rewritten as:

A(f(θ)-x _d )=r; (10)

表示描述运动的参数矩阵。The control objective is described by designing joints so that r=r _d ; ∑ _t represents the parameter matrix describing the contact;

Represents a matrix of parameters describing motion.

Optionally, the constructing an equation constraint for realizing compliance force control based on the current state feedback information includes:

The current state feedback information obtained under the robot motion modeling system, for a given desired trajectory x _d and contact force F _t , the goal of achieving position-to-control is:

Then define the error vector:

e=rr _d =[FF _d ; e ₀ ]; (16)

Then the equation can be reconstructed in the velocity layer as follows:

表示所述机器人的关节角速度；

表示误差向量的一阶导数；

表示期望轨迹的一阶导数；

表示r_d的一阶导数，r_d＝[F_d；0]，F_d表示机器人的力指令。Among them, k represents a positive control constant;

represents the joint angular velocity of the robot;

represents the first derivative of the error vector;

represents the first derivative of the desired trajectory;

Represents the first derivative of r _d , r _d =[F _d ; 0], F _d represents the force command of the robot.

Optionally, the inequality constraints of joint angles, joint angular velocities and joint moments in the robot system include:

其中，

The inequality constraint normalization is described as the inequality constraint of the velocity layer:

in,

Then the inequality constraints for joint moments can be rewritten as:

Among them, β>0, when the contact force between the end effector and the workpiece under the base frame R ₀ (x ₀ , y ₀ , z ₀ ) is F, the expression of the acting torque applied at each joint The derivative can be obtained by:

By combining formulas (18) and (19), the description of the joint moment in the angular velocity layer can be obtained:

J表示Jacobian矩阵；

表示关节力矩约束的一阶导数；τ表示关节力矩约束；β表示正控制参数；θ表示机器人的关节角度；

表示机器人的关节角速度；H表示一个实数数组；

表示所述机器人的指令力的一阶导数；

表示实数。in,

J represents the Jacobian matrix;

represents the first derivative of the joint moment constraint; τ represents the joint moment constraint; β represents the positive control parameter; θ represents the joint angle of the robot;

Represents the joint angular velocity of the robot; H represents a real number array;

represents the first derivative of the command force of the robot;

represents a real number.

Optionally, the joint moment function is rewritten to obtain a final constraint optimization model, including:

Simplify the objective function and use the command force F _d of the robot to replace the contact force F under the base frame R ₀ (x ₀ , y ₀ , z ₀ ), there are:

因此通过求取G₂对θ的梯度，得到其在速度层上的替代描述：If the objective function described by formula (13) is defined in the joint angle layer, since the final control quantity is the joint angular velocity

Therefore, by taking the gradient of G2 to θ, its alternative description _on the velocity layer is obtained:

Derivation with respect to J ^T (θ)F _d yields:

是：in,

Yes:

Let H=[H ₁ ,...,H _n ], the above formula can describe the number as:

不相关，则选择最终目标函数选取为

Since the second term in equation (15) is the same as

is not relevant, then the final objective function is selected as

对目标函数中的

进行凸化处理，则最终的约束优化模型为：by introducing a modifier

in the objective function

After convexization, the final constrained optimization model is:

表示所述机器人的指令力的转秩；J表示Jacobian矩阵；

表示机器人的关节角速度；r_r表示参考指令；A表示简写的矩阵。in,

Represents the rotation rank of the command force of the robot; J represents the Jacobian matrix;

Represents the joint angular velocity of the robot; r _r represents the reference command; A represents the abbreviated matrix.

Optionally, the state variables and control torques in the final constrained optimization model are updated based on the dynamic neural network model, including:

为所述约束优化模型中的对偶状态变量，则拉格朗日函数选取为：Define input state variables

For the dual state variables in the constrained optimization model, the Lagrangian function is selected as:

According to the Karush-Kuhn-Tucker condition, optimizing the optimal solution in the constrained optimization model is equivalently expressed as:

where P _Ω ( ) is a clipping function defined as:

为一个投影函数，定义为：

is a projection function, defined as:

When solving equation (24) in real time, the position-force controller design based on the dynamic neural network model is designed as:

表示实数；

表示输入状态量λ₂的一阶导数；

表示输入状态量λ₁的一阶导数；

表示关节角速度；

表示关节角加速度；J表示Jacobian矩阵；r_r表示参考指令；A表示简写矩阵；g₁表示第一个元素；g_2m表示第2m个元素；∈表示一个正常数。in,

represents a real number;

represents the first derivative of the input state quantity λ ₂ ;

represents the first derivative of the input state quantity λ ₁ ;

represents the joint angular velocity;

represents the joint angular acceleration; J represents the Jacobian matrix; r _r represents the reference command; A represents the abbreviated matrix; g ₁ represents the first element; g _2m represents the 2mth element; ∈ represents a positive number.

In addition, an embodiment of the present invention also provides a compliance force control system for a collaborative robot, the system comprising:

Initialization module: used to obtain and initialize the state variables of the robot;

Obtaining module: used to obtain the current rotation matrix based on the initialization state variable;

Reading module: used to read the current state feedback information of the robot based on the current rotation matrix;

Building module: used to construct, based on the current state feedback information, equality constraints for realizing compliance force control and inequality constraints for joint angles, joint angular velocities and joint moments in the robot system;

Rewriting module: used to rewrite the joint moment function and obtain the final constraint optimization model;

Update module: used to update the state variables and control torques in the final constrained optimization model based on the dynamic neural network model;

Judging module: used to judge whether the current time is greater than the task time, if so, end the compliance control of the robot, otherwise, return to obtain the current rotation matrix based on the initialization state variable.

In the embodiment of the present invention, through the real-time method of the present invention, high-precision force control in the direction of contact force and motion control in the direction of free motion can be realized simultaneously; online optimization of joint torque can be realized; and in the process of compliance force control guarantees that the robot does not exceed its physical constraints.

Description of drawings

In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the accompanying drawings that need to be used in the description of the embodiments or the prior art. Obviously, the accompanying drawings in the following description are only These are some embodiments of the present invention, and for those of ordinary skill in the art, other drawings can also be obtained from these drawings without creative effort.

1 is a schematic flowchart of a method for controlling compliance force of a collaborative robot in an embodiment of the present invention;

FIG. 2 is a schematic diagram of a robot position-force control in an embodiment of the present invention;

FIG. 3 is a schematic structural composition diagram of a compliance force control system for a collaborative robot in an embodiment of the present invention.

Detailed ways

The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, but not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

Example

Please refer to FIG. 1 . FIG. 1 is a schematic flowchart of a method for controlling compliance force of a collaborative robot according to an embodiment of the present invention.

As shown in Fig. 1, a method for controlling the compliance force of a collaborative robot, the method includes:

S11: Obtain the state variable of the robot and initialize it;

In the specific implementation process of the present invention, before the state variable of the robot is obtained and initialized, the method further includes: according to the orthogonal characteristics of the contact force between the robot motion and the workpiece, in the tool coordinate system and the base coordinate system Perform separate modeling to obtain a robot motion modeling system; wherein, the base coordinate system represents R ₀ (x ₀ , y ₀ , z ₀ ); the tool coordinate system represents R _t (x _t , y _t , z _t ) .

Further, the contact force between the robot and the workpiece is parallel to z _t in the tool coordinate system, while x _t and y _t define the free movement of the robot end effector;

During the operation of the robot, there is a slight deviation between the actual position _x of the _robot end _effector and the expected trajectory _{x d} . The coordinate system R _t (x _t , y _t , z _t ) is described as the deviation δX of the robot from the desired trajectory under the base frame and the deviation δX of the robot from the desired trajectory under the tool coordinate system, respectively _t .

Further, separately modeling is performed in the tool coordinate system and the base coordinate system, and the process of obtaining the robot motion modeling system is as follows:

In the tool coordinate system R _t (x _t , y _t , z _t ), since the friction between the robot and the workpiece is ignored, assuming that the contact between the robot and the workpiece is a rigid contact, then The contact force can be described as:

F _t =k _f ∑ _t δX _t ; (1)

Among them, k _f represents the stiffness coefficient; ∑ _t =diag(0,0,1) is a diagonal matrix, which is used to describe the deviation δX _t of the robot from the desired trajectory in the tool coordinate system and its contact force. relationship, where 0 means that the displacement in this direction will not generate contact force, otherwise, take 1;

则在所述工具坐标系R_t(x_t,y_t,z_t)下的位置跟踪误差e_t为：definition

Then the position tracking error e _t under the tool coordinate system R _t (x _t , y _t , z _t ) is:

When the contact surface is known, a rotation matrix S _t is used to describe the tool coordinate system R _t (x _t , y _t , z _t ) and the base frame R ₀ (x ₀ , y ₀ , z ₀ ) The rotation relationship between;

Define F and e ₀ as the corresponding descriptions of δX _t and F _t under the base frame R ₀ (x ₀ , y ₀ , z ₀ ), respectively, then there are:

e _t =S _t e ₀ ; (4)

δX _t =S _t δX; (5)

Through the combination of formulas (1)-(5), we have:

Wherein, δX represents the deviation of the robot from the expected trajectory under the base frame R ₀ (x ₀ , y ₀ , z ₀ ); F represents the base frame R ₀ (x ₀ , y ₀ , contact force at z ₀ );

The displacement δX in the base frame R ₀ (x ₀ , y ₀ , z ₀ ) can be described as δX=xx _d , where the desired trajectory x _d is the desired position signal described in R ₀ , therefore, for formula (6 )-(7) is rewritten as:

表示描述运动的参数矩阵。Define the desired trajectory and command force of the robot as x _d and F _d respectively; then according to the description of formula (5) and formula (9), the control objective can be described as designing a position-oriented control strategy for redundant robots, so that Contact force F→F _d described by formula (8), while making the tracking error e ₀ described by formula (9) → 0; ∑ _t represents the parameter matrix describing the contact;

Represents a matrix of parameters describing motion.

r_d＝[F_d；0]，

则有公式(8)和公式(9)重写为：Further, in order to simplify the description in the robot motion modeling system, the definition

r _d =[F _d ; 0],

Then there are formulas (8) and (9) rewritten as:

A(f(θ)-x _d )=r; (10)

表示描述运动的参数矩阵。The control objective is described by designing joints so that r=r _d ; ∑ _t represents the parameter matrix describing the contact;

Represents a matrix of parameters describing motion.

Specifically, according to the orthogonal characteristics of robot motion and contact force, _modeling is _carried out in the tool _coordinate system and the base coordinate system. z _t ) and the base frame R ₀ (x ₀ , y ₀ , z ₀ ), as shown in Figure 2, the contact force between the robot and the workpiece is parallel to z _t in the tool coordinate system, while x _t and y _t define the free motion of the robot end effector; during the operation, there is a slight deviation between the actual position x of the robot end effector and the expected trajectory x _d , and in the base frame R ₀ (x ₀ , y ₀ , z ₀ ) and the tool coordinate system R _t (x _t , y _t , z _t ) are respectively described as the deviation δX of the robot from the desired trajectory in the base coordinate system and the robot’s deviation from the desired trajectory in the tool coordinate system Deviation δX _t .

In the tool coordinate system R _t (x _t , y _t , z _t ), since the friction between the robot and the workpiece is ignored, assuming that the contact between the robot and the workpiece is a rigid contact, the contact force can be described as:

F _t =k _f ∑ _t δX _t ; (1)

Among them, k _f represents the stiffness coefficient; ∑ _t =diag(0,0,1) is a diagonal matrix, which is used to describe the deviation δX _t of the robot from the desired trajectory in the tool coordinate system and its contact force. relationship, where 0 means that the displacement in this direction will not produce contact force, otherwise, take 1.

则在工具坐标系R_t(x_t,y_t,z_t)下的位置跟踪误差e_t为：definition

Then the position tracking error e _t under the tool coordinate system R _t (x _t , y _t , z _t ) is:

When the contact surface is known, a rotation matrix S _t is used to describe the relationship between the tool coordinate system R _t (x _t ,y _t ,z _t ) and the base frame R ₀ (x ₀ ,y ₀ ,z ₀ ) rotation relationship.

Define F and e ₀ as the corresponding descriptions of δX _t and F _t under the base frame R ₀ (x ₀ , y ₀ , z ₀ ), respectively, then we have:

e _t =S _t e ₀ ; (4)

δX _t =S _t δX; (5)

Through the combination of formulas (1)-(5), we have:

Among them, δX represents the deviation of the robot from the expected trajectory under the base frame R ₀ (x ₀ , y ₀ , z ₀ ); F represents the base frame R ₀ (x ₀ , y ₀ , z _{0 )} ) under the contact force.

It is worth noting that the displacement δX in the base frame R ₀ (x ₀ , y ₀ , z ₀ ) can be described as δX=xx _d , where the desired trajectory x _d is the desired position signal described in R ₀ , so , the formulas (6)-(7) are rewritten as:

表示描述运动的参数矩阵。Define the desired trajectory and command force of the robot as x _d and F _d respectively; then according to the description of formula (5) and formula (9), the control objective can be described as designing a position-oriented control strategy for redundant robots, so that Contact force F→F _d described by formula (8), while making the tracking error e ₀ described by formula (9) → 0; ∑ _t represents the parameter matrix describing the contact;

Represents a matrix of parameters describing motion.

r_d＝[F_d；0]，

则有公式(8)和公式(9)重写为：To simplify the description, define

r _d =[F _d ; 0],

Then there are formulas (8) and (9) rewritten as:

A(f(θ)-x _d )=r; (10)

表示描述运动的参数矩阵。The control objective is described by designing joints so that r=r _d ; ∑ _t represents the parameter matrix describing the contact;

Represents a matrix of parameters describing motion.

After building the system modeling model of the robot, then the state variables of the robot are obtained, and then the corresponding initialization is performed.

S12: Obtain the current rotation matrix based on the initialization state variable;

In the specific implementation process of the present invention, after the initialization state variable is obtained, the current rotation matrix is obtained according to the initialization state variable; that is, the rotation matrix S _t in the modeled robot system.

S13: Read the current state feedback information of the robot based on the current rotation matrix;

In the specific implementation process of the present invention, the feedback information of the robot state feedback is obtained through the current rotation matrix, that is, the state feedback information obtained through the robot system includes the state feedback information under the base frame R ₀ (x ₀ , y ₀ , z ₀ ). Contact force, joint angular velocity of the robot, and joint angle of the robot.

S14: Constructing equality constraints for realizing compliance force control and inequality constraints for joint angles, joint angular velocities and joint moments in the robot system based on the current state feedback information;

In the specific implementation process of the present invention, the constructing an equation constraint for realizing compliance force control based on the current state feedback information includes: the current state feedback information obtained under the robot motion modeling system, for a given desired trajectory x _d and contact force F _t , the goal of achieving position-to-control is:

Then define the error vector:

e=rr _d =[FF _d ; e ₀ ]; (16)

Then the equation can be reconstructed in the velocity layer as follows:

表示所述机器人的关节角速度；

表示误差向量的一阶导数；

表示期望轨迹的一阶导数；

表示r_d的一阶导数，r_d＝[F_d；0]，F_d表示机器人的力指令。Among them, k represents a positive control constant;

represents the joint angular velocity of the robot;

represents the first derivative of the error vector;

represents the first derivative of the desired trajectory;

Represents the first derivative of r _d , r _d =[F _d ; 0], F _d represents the force command of the robot.

其中，

Further, the inequality constraints of joint angles, joint angular velocities and joint moments in the robot system include: normalizing the inequality constraints to describe the inequality constraints of the velocity layer:

in,

Then the inequality constraints for joint moments can be rewritten as:

Among them, β>0, when the contact force between the end effector and the workpiece under the base frame R ₀ (x ₀ , y ₀ , z ₀ ) is F, the expression of the acting torque applied by the end effector at each joint The derivative can be obtained by:

By combining formulas (18) and (19), the description of the joint moment in the angular velocity layer can be obtained:

J表示Jacobian矩阵；

表示关节力矩约束的一阶导数；τ表示关节力矩约束；β表示正控制参数；θ表示机器人的关节角度；

表示机器人的关节角速度；H表示一个实数数组；

表示所述机器人的指令力的一阶导数；

表示实数。in,

J represents the Jacobian matrix;

represents the first derivative of the joint moment constraint; τ represents the joint moment constraint; β represents the positive control parameter; θ represents the joint angle of the robot;

Represents the joint angular velocity of the robot; H represents a real number array;

represents the first derivative of the command force of the robot;

represents a real number.

First, the basic QP problem description is carried out; when the contact force between the end effector and the workpiece is F, the applied torque at each joint is:

τ=J ^T (θ)F; (11)

From the perspective of energy saving, the objective function is selected as τ ^T τ/2 to describe the energy consumption of the system; at the same time, when the contact force F is large, in order to avoid the safety risk caused by excessive torque on a joint, the joint angle constraint , on the basis of the angular velocity constraint, the joint torque constraint τ ^min ≤τ≤τ ^max is introduced; then the position-force control problem of the redundant robot is described as the following QP problem:

min G ₁ =F ^T J(θ)J ^T (θ)F/2; (12a)

str _d =A(f(θ)-x _d ); (12b)

θ ^min ≤θ≤θ ^max ; (12c)

τ ^min ≤JT(θ)F _d ≤τ ^max ; (12e)

Then carry out the constraint reconstruction of equations and inequalities; according to the above formula (10) and the definition of r _d , for a given desired trajectory x _d and command force F _d , the goal of achieving position-force control is:

Then define the error vector:

e=rr _d =[FF _d ; e ₀ ]; (16)

Then the equation can be reconstructed in the velocity layer as follows:

表示所述机器人的关节角速度；

表示误差向量的一阶导数；

表示期望轨迹的一阶导数；

表示r_d的一阶导数，r_d＝[F_d；0]，F_d表示机器人的力指令。Among them, k represents a positive control constant;

represents the joint angular velocity of the robot;

represents the first derivative of the error vector;

represents the first derivative of the desired trajectory;

Represents the first derivative of r _d , r _d =[F _d ; 0], F _d represents the force command of the robot.

其中，

For the above inequality constraints (12c) and (12d), with reference to the processing method in the above, the inequality constraint normalization is described as the inequality constraint of the velocity layer:

in,

The inequality constraint (12e) of the same joint moment can be rewritten as:

Among them, β>0, when the contact force between the end effector and the workpiece under the base frame R ₀ (x ₀ , y ₀ , z ₀ ) is F, the equation (11) applies to each joint. The derivation of the expression of the acting moment can be obtained:

By combining formulas (18) and (19), the description of the joint moment in the angular velocity layer can be obtained:

J表示Jacobian矩阵；

表示关节力矩约束的一阶导数；τ表示关节力矩约束；β表示正控制参数；θ表示机器人的关节角度；

表示机器人的关节角速度；H表示一个实数数组；

表示所述机器人的指令力的一阶导数；

表示实数。in,

J represents the Jacobian matrix;

represents the first derivative of the joint moment constraint; τ represents the joint moment constraint; β represents the positive control parameter; θ represents the joint angle of the robot;

Represents the joint angular velocity of the robot; H represents a real number array;

represents the first derivative of the command force of the robot;

represents a real number.

In summary, the redundant robot position-force control problem based on the constraint-optimization idea is described in the angular velocity layer as follows:

in,

S15: Rewrite the joint moment function and obtain the final constraint optimization model;

In the specific implementation process of the present invention, the joint torque function is rewritten, and the final constraint optimization model is obtained, including:

Simplify the objective function and use the command force F _d of the robot to replace the contact force F under the base frame R ₀ (x ₀ , y ₀ , z ₀ ), there are:

因此通过求取G₂对θ的梯度，得到其在速度层上的替代描述：If the objective function described by formula (13) is defined in the joint angle layer, since the final control quantity is the joint angular velocity

Therefore, by taking the gradient of G2 to θ, its alternative description _on the velocity layer is obtained:

Derivation with respect to J ^T (θ)F _d yields:

是：in,

Yes:

Let H=[H ₁ ,...,H _n ], the above formula can describe the number as:

不相关，则选择最终目标函数选取为

Since the second term in equation (15) is the same as

is not relevant, then the final objective function is selected as

对目标函数中的

进行凸化处理，则最终的约束优化模型为：by introducing a modifier

in the objective function

After convexization, the final constrained optimization model is:

表示所述机器人的指令力的转秩；J表示Jacobian矩阵；

表示机器人的关节角速度；r_r表示参考指令；A表示简写的矩阵。in,

Represents the rotation rank of the command force of the robot; J represents the Jacobian matrix;

Represents the joint angular velocity of the robot; r _r represents the reference command; A represents the abbreviated matrix.

这使后续控制器设计变得困难，因此将该目标函数进行简化：使用机器人的指令力F_d代替在基座标系R₀(x₀,y₀,z₀)下的接触力F，则有：Specifically, a large number of nonlinear features are included in formula (12), including the Jacobian matrix, and the real-time contact force

This makes the subsequent controller design difficult, so the objective function is simplified: use the command force F _d of the robot to replace the contact force F under the base frame R ₀ (x ₀ , y ₀ , z ₀ ), then Have:

因此通过求取G₂对θ的梯度，得到其在速度层上的替代描述：When F _d is not related to θ, using F _d can greatly reduce the nonlinearity of the objective function; on the other hand, with a reasonable controller design, the contact force F will eventually converge to F _d , so the objective function before and after replacement is finally Equivalently, the objective function described by formula (13) is defined in the joint angle layer, since the final control quantity is the joint angular velocity

Therefore, by taking the gradient of G2 to θ, its alternative description _on the velocity layer is obtained:

Derivation with respect to J ^T (θ)F _d yields:

是：in,

Yes:

Let H=[H ₁ ,...,H _n ], the above formula can describe the number as:

不相关，则选择最终目标函数选取为

Since the second term in equation (15) is the same as

is not relevant, then the final objective function is selected as

而言是非凸的，通过引入一个修正项

对目标函数中的

进行凸化处理，则最终的约束优化模型为：Since the objective function described by Eq. (21a) is

is non-convex in terms of

in the objective function

After convexization, the final constrained optimization model is:

表示所述机器人的指令力的转秩；J表示Jacobian矩阵；

表示机器人的关节角速度；r_r表示参考指令；A表示简写的矩阵。in,

Represents the rotation rank of the command force of the robot; J represents the Jacobian matrix;

Represents the joint angular velocity of the robot; r _r represents the reference command; A represents the abbreviated matrix.

S16: Update the state variables and control torques in the final constrained optimization model based on the dynamic neural network model;

为所述约束优化模型中的对偶状态变量，则拉格朗日函数选取为：In the specific implementation process of the present invention, the updating of the state variables and control torques in the final constrained optimization model based on the dynamic neural network model includes: defining input state variables

For the dual state variables in the constrained optimization model, the Lagrangian function is selected as:

According to the Karush-Kuhn-Tucker condition, optimizing the optimal solution in the constrained optimization model is equivalently expressed as:

where P _Ω ( ) is a clipping function defined as:

为一个投影函数，定义为：

is a projection function, defined as:

When solving equation (24) in real time, the position-force controller design based on the dynamic neural network model is designed as:

表示实数；

表示输入状态量λ₂的一阶导数；

表示输入状态量λ₁的一阶导数；

表示关节角速度；

表示关节角加速度；J表示Jacobian矩阵；r_r表示参考指令；A表示简写矩阵；g₁表示第一个元素；g_2m表示第2m个元素；∈表示一个正常数。in,

represents a real number;

represents the first derivative of the input state quantity λ ₂ ;

represents the first derivative of the input state quantity λ ₁ ;

represents the joint angular velocity;

represents the joint angular acceleration; J represents the Jacobian matrix; r _r represents the reference command; A represents the abbreviated matrix; g ₁ represents the first element; g _2m represents the 2mth element; ∈ represents a positive number.

为约束公式(22b)和(22c)的对偶状态变量，则拉格朗日函数选取为：Specifically, define the input state variable

is the dual state variable of constraint formulas (22b) and (22c), then the Lagrangian function is selected as:

According to the Karush-Kuhn-Tucker condition, optimizing the optimal solution in the constrained optimization model is equivalently expressed as:

where P _Ω ( ) is a clipping function defined as:

为一个投影函数，定义为：

is a projection function, defined as:

When solving equation (24) in real time, the position-force controller design based on the dynamic neural network model is designed as:

表示实数；

表示输入状态量λ₂的一阶导数；

表示输入状态量λ₁的一阶导数；

表示关节角速度；

表示关节角加速度；J表示Jacobian矩阵；r_r表示参考指令；A表示简写矩阵；g₁表示第一个元素；g_2m表示第2m个元素；∈表示一个正常数。in,

represents a real number;

represents the first derivative of the input state quantity λ ₂ ;

represents the first derivative of the input state quantity λ ₁ ;

represents the joint angular velocity;

represents the joint angular acceleration; J represents the Jacobian matrix; r _r represents the reference command; A represents the abbreviated matrix; g ₁ represents the first element; g _2m represents the 2mth element; ∈ represents a positive number.

S17: Determine whether the current time is greater than the task time;

In the specific implementation process of the present invention, it is necessary to judge whether the current time is greater than the task time, and when it is greater than the task time, execute S18, otherwise, return to S12.

S18: If yes, end the compliance force control of the robot, otherwise, return to obtain the current rotation matrix based on the initialization state variable.

In the specific implementation process of the present invention, when it is judged that the current time is greater than the task time, the compliance force control of the robot is ended.

In the embodiment of the present invention, through the real-time method of the present invention, high-precision force control in the direction of contact force and motion control in the direction of free motion can be realized simultaneously; online optimization of joint torque can be realized; and in the process of compliance force control guarantees that the robot does not exceed its physical constraints.

Example

Please refer to FIG. 3 . FIG. 3 is a schematic structural diagram of a compliance force control system for a collaborative robot according to an embodiment of the present invention.

As shown in Figure 3, a collaborative robot compliance force control system, the system includes:

Initialization module 21: used to obtain and initialize the state variables of the robot;

In the specific implementation process of the present invention, before the state variable of the robot is obtained and initialized, the method further includes: according to the orthogonal characteristics of the contact force between the robot motion and the workpiece, in the tool coordinate system and the base coordinate system Perform separate modeling to obtain a robot motion modeling system; wherein, the base coordinate system represents R ₀ (x ₀ , y ₀ , z ₀ ); the tool coordinate system represents R _t (x _t , y _t , z _t ) .

Further, the contact force between the robot and the workpiece is parallel to z _t in the tool coordinate system, while x _t and y _t define the free movement of the robot end effector;

During the operation of the robot, there is a slight deviation between the actual position _x of the _robot end _effector and the expected trajectory _{x d} . The coordinate system R _t (x _t , y _t , z _t ) is described as the deviation δX of the robot from the desired trajectory under the base frame and the deviation δX of the robot from the desired trajectory under the tool coordinate system, respectively _t .

Further, separately modeling is performed in the tool coordinate system and the base coordinate system, and the process of obtaining the robot motion modeling system is as follows:

In the tool coordinate system R _t (x _t , y _t , z _t ), since the friction between the robot and the workpiece is ignored, assuming that the contact between the robot and the workpiece is a rigid contact, then The contact force can be described as:

F _t =k _f ∑ _t δX _t ; (1)

Among them, k _f represents the stiffness coefficient; ∑ _t =diag(0,0,1) is a diagonal matrix, which is used to describe the deviation δX _t of the robot from the desired trajectory in the tool coordinate system and its contact force. relationship, where 0 means that the displacement in this direction will not generate contact force, otherwise, take 1;

则在所述工具坐标系R_t(x_t,y_t,z_t)下的位置跟踪误差e_t为：definition

Then the position tracking error e _t under the tool coordinate system R _t (x _t , y _t , z _t ) is:

When the contact surface is known, a rotation matrix S _t is used to describe the tool coordinate system R _t (x _t , y _t , z _t ) and the base frame R ₀ (x ₀ , y ₀ , z ₀ ) The rotation relationship between;

Define F and e ₀ as the corresponding descriptions of δX _t and F _t under the base frame R ₀ (x ₀ , y ₀ , z ₀ ), respectively, then there are:

e _t =S _t e ₀ ; (4)

δX _t =S _t δX; (5)

Through the combination of formulas (1)-(5), we have:

Wherein, δX represents the deviation of the robot from the expected trajectory under the base frame R ₀ (x ₀ , y ₀ , z ₀ ); F represents the base frame R ₀ (x ₀ , y ₀ , contact force at z ₀ );

The displacement δX in the base frame R ₀ (x ₀ , y ₀ , z ₀ ) can be described as δX=xx _d , where the desired trajectory x _d is the desired position signal described in R ₀ , therefore, for formula (6 )-(7) is rewritten as:

表示描述运动的参数矩阵。Define the desired trajectory and command force of the robot as x _d and F _d respectively; then according to the description of formula (5) and formula (9), the control objective can be described as designing a position-oriented control strategy for redundant robots, so that Contact force F→F _d described by formula (8), while making the tracking error e ₀ described by formula (9) → 0; ∑ _t represents the parameter matrix describing the contact;

Represents a matrix of parameters describing motion.

r_d＝[F_d；0]，

则有公式(8)和公式(9)重写为：Further, in order to simplify the description in the robot motion modeling system, the definition

r _d =[F _d ; 0],

Then there are formulas (8) and (9) rewritten as:

A(f(θ)-x _d )=r; (10)

表示描述运动的参数矩阵。The control objective is described by designing joints so that r=r _d ; ∑ _t represents the parameter matrix describing the contact;

Represents a matrix of parameters describing motion.

Specifically, according to the orthogonal characteristics of robot motion and contact force, _modeling is _carried out in the tool _coordinate system and the base coordinate system. z _t ) and the base frame R ₀ (x ₀ , y ₀ , z ₀ ), as shown in Figure 2, the contact force between the robot and the workpiece is parallel to z _t in the tool coordinate system, while x _t and y _t define the free motion of the robot end effector; during the operation, there is a slight deviation between the actual position x of the robot end effector and the expected trajectory x _d , and in the base frame R ₀ (x ₀ , y ₀ , z ₀ ) and the tool coordinate system R _t (x _t , y _t , z _t ) are respectively described as the deviation δX of the robot from the desired trajectory in the base coordinate system and the robot’s deviation from the desired trajectory in the tool coordinate system Deviation δX _t .

In the tool coordinate system R _t (x _t , y _t , z _t ), since the friction between the robot and the workpiece is ignored, assuming that the contact between the robot and the workpiece is a rigid contact, the contact force can be described as:

F _t =k _f ∑ _t δX _t ; (1)

Among them, k _f represents the stiffness coefficient; ∑ _t =diag(0,0,1) is a diagonal matrix, which is used to describe the deviation δX _t of the robot from the desired trajectory in the tool coordinate system and its contact force. relationship, where 0 means that the displacement in this direction will not produce contact force, otherwise, take 1.

则在工具坐标系R_t(x_t,y_t,z_t)下的位置跟踪误差e_t为：definition

Then the position tracking error e _t under the tool coordinate system R _t (x _t , y _t , z _t ) is:

When the contact surface is known, a rotation matrix S _t is used to describe the relationship between the tool coordinate system R _t (x _t ,y _t ,z _t ) and the base frame R ₀ (x ₀ ,y ₀ ,z ₀ ) rotation relationship.

Define F and e ₀ as the corresponding descriptions of δX _t and F _t under the base frame R ₀ (x ₀ , y ₀ , x ₀ ), respectively, then there are:

e _t =S _t e ₀ ; (4)

δX _t =S _t δX; (5)

Through the combination of formulas (1)-(5), we have:

Among them, δX represents the deviation of the robot from the expected trajectory under the base frame R ₀ (x ₀ , y ₀ , z ₀ ); F represents the base frame R ₀ (x ₀ , y ₀ , z _{0 )} ) under the contact force.

It is worth noting that the displacement δX in the base frame T ₀ (x ₀ , y ₀ , z ₀ ) can be described as δX=xx _d , where the desired trajectory x _d is the desired position signal described in R ₀ , so , the formulas (6)-(7) are rewritten as:

表示描述运动的参数矩阵。Define the desired trajectory and command force of the robot as x _d and F _d respectively; then according to the description of formula (5) and formula (9), the control objective can be described as designing a position-oriented control strategy for redundant robots, so that Contact force F→F _d described by formula (8), while making the tracking error e ₀ described by formula (9) → 0; ∑ _t represents the parameter matrix describing the contact;

Represents a matrix of parameters describing motion.

r_d＝[F_d；0]，

则有公式(8)和公式(9)重写为：To simplify the description, define

r _d =[F _d ; 0],

Then there are formulas (8) and (9) rewritten as:

A(f(θ)-x _d )=r; (10)

表示描述运动的参数矩阵。The control objective is described by designing joints so that r=r _d ; ∑ _t represents the parameter matrix describing the contact;

Represents a matrix of parameters describing motion.

After building the system modeling model of the robot, then the state variables of the robot are obtained, and then the corresponding initialization is performed.

Obtaining module 22: used to obtain the current rotation matrix based on the initialization state variable;

In the specific implementation process of the present invention, after the initialization state variable is obtained, the current rotation matrix is obtained according to the initialization state variable; that is, the rotation matrix St in the modeled robot system.

Reading module 23: for reading the current state feedback information of the robot based on the current rotation matrix;

In the specific implementation process of the present invention, the feedback information of the robot state feedback is obtained through the current rotation matrix, that is, the state feedback information obtained through the robot system includes the state feedback information under the base frame R ₀ (x ₀ , y ₀ , z ₀ ). Contact force, joint angular velocity of the robot, and joint angle of the robot.

Building module 24: used to construct, based on the current state feedback information, an equation constraint for realizing compliance force control and an inequality constraint for joint angles, joint angular velocities and joint moments in the robot system;

In the specific implementation process of the present invention, the constructing an equation constraint for realizing compliance force control based on the current state feedback information includes: the current state feedback information obtained under the robot motion modeling system, for a given desired trajectory x _d and contact force F _t , the goal of achieving position-to-control is:

Then define the error vector:

e=rr _d =[FF _d ; e ₀ ]; (16)

Then the equation can be reconstructed in the velocity layer as follows:

表示所述机器人的关节角速度；

表示误差向量的一阶导数；

表示期望轨迹的一阶导数；

表示r_d的一阶导数，r_d＝[F_d；0]，F_d表示机器人的力指令。Among them, k represents a positive control constant;

represents the joint angular velocity of the robot;

represents the first derivative of the error vector;

represents the first derivative of the desired trajectory;

Represents the first derivative of r _d , r _d =[F _d ; 0], F _d represents the force command of the robot.

其中，

Further, the inequality constraints of the joint angles, joint angular velocities and joint moments in the robot system include: normalizing the inequality constraints to describe the inequality constraints of the velocity layer:

in,

Then the inequality constraints for joint moments can be rewritten as:

Among them, β>0, when the contact force between the end effector and the workpiece under the base frame R ₀ (x ₀ , y ₀ , z ₀ ) is F, the expression of the acting torque applied at each joint The derivative can be obtained by:

By combining formulas (18) and (19), the description of the joint moment in the angular velocity layer can be obtained:

J表示Jacobian矩阵；

表示关节力矩约束的一阶导数；τ表示关节力矩约束；β表示正控制参数；θ表示机器人的关节角度；

表示机器人的关节角速度；H表示一个实数数组；

表示所述机器人的指令力的一阶导数；

表示实数。in,

J represents the Jacobian matrix;

represents the first derivative of the joint moment constraint; τ represents the joint moment constraint; β represents the positive control parameter; θ represents the joint angle of the robot;

Represents the joint angular velocity of the robot; H represents a real number array;

represents the first derivative of the command force of the robot;

represents a real number.

First, the basic QP problem description is carried out; when the contact force between the end effector and the workpiece is F, the applied torque at each joint is:

τ=J ^T (θ)F; (11)

From the perspective of energy saving, the objective function is selected as τ ^T τ/2 to describe the energy consumption of the system; at the same time, when the contact force F is large, in order to avoid the safety risk caused by excessive torque on a joint, the joint angle constraint , on the basis of the angular velocity constraint, the joint torque constraint τ ^min ≤τ≤τ ^max is introduced; then the position-force control problem of the redundant robot is described as the following QP problem:

minG ₁ =F ^T J(θ)J ^T (θ)F/2; (12a)

str _d =A(f(θ)-x _d ); (12b)

θ ^min ≤θ≤θ ^max ; (12c)

τ ^min ≤J ^T (θ)F _d ≤τ ^max ; (12e)

Then carry out the constraint reconstruction of equations and inequalities; according to the above formula (10) and the definition of r _d , for a given desired trajectory x _d and command force F _d , the goal of achieving position-force control is:

Then define the error vector:

e=rr _d =[FF _d ; e ₀ ]; (16)

Then the equation can be reconstructed in the velocity layer as follows:

表示所述机器人的关节角速度；

表示误差向量的一阶导数；

表示期望轨迹的一阶导数；

表示r_d的一阶导数，r_d＝[F_d；0]，F_d表示机器人的力指令。Among them, k represents a positive control constant;

represents the joint angular velocity of the robot;

represents the first derivative of the error vector;

represents the first derivative of the desired trajectory;

Represents the first derivative of r _d , r _d =[F _d ; 0], F _d represents the force command of the robot.

其中，

For the above inequality constraints (12c) and (12d), with reference to the processing method in the above, the inequality constraint normalization is described as the inequality constraint of the velocity layer:

in,

The inequality constraint (12e) of the same joint moment can be rewritten as:

Among them, β>0, when the contact force between the end effector and the workpiece under the base frame R ₀ (x ₀ , y ₀ , z ₀ ) is F, the equation (11) applies to each joint. The derivation of the expression of the acting moment can be obtained:

By combining formulas (18) and (19), the description of the joint moment in the angular velocity layer can be obtained:

J表示Jacobian矩阵；

表示关节力矩约束的一阶导数；τ表示关节力矩约束；β表示正控制参数；θ表示机器人的关节角度；

表示机器人的关节角速度；H表示一个实数数组；

表示所述机器人的指令力的一阶导数；

表示实数。in,

J represents the Jacobian matrix;

represents the first derivative of the joint moment constraint; τ represents the joint moment constraint; β represents the positive control parameter; θ represents the joint angle of the robot;

Represents the joint angular velocity of the robot; H represents a real number array;

represents the first derivative of the command force of the robot;

represents a real number.

In summary, the redundant robot position-force control problem based on the constraint-optimization idea is described in the angular velocity layer as follows:

in,

Rewriting module 25: used to rewrite the joint moment function and obtain the final constraint optimization model;

In the specific implementation process of the present invention, the joint torque function is rewritten, and the final constraint optimization model is obtained, including:

Simplify the objective function and use the command force F _d of the robot to replace the contact force F under the base frame R ₀ (x ₀ , y ₀ , z ₀ ), there are:

因此通过求取G₂对θ的梯度，得到其在速度层上的替代描述：If the objective function described by formula (13) is defined in the joint angle layer, since the final control quantity is the joint angular velocity

Therefore, by taking the gradient of G2 to θ, its alternative description _on the velocity layer is obtained:

Derivation with respect to J ^T (θ)F _d yields:

是：in,

Yes:

Let H=[H ₁ ,...,H _n ], the above formula can describe the number as:

不相关，则选择最终目标函数选取为

Since the second term in equation (15) is the same as

is not relevant, then the final objective function is selected as

对目标函数中的

进行凸化处理，则最终的约束优化模型为：by introducing a modifier

in the objective function

After convexization, the final constrained optimization model is:

表示所述机器人的指令力的转秩；J表示Jacobian矩阵；

表示机器人的关节角速度；r_r表示参考指令；A表示简写的矩阵。in,

Represents the rotation rank of the command force of the robot; J represents the Jacobian matrix;

Represents the joint angular velocity of the robot; r _r represents the reference command; A represents the abbreviated matrix.

这使后续控制器设计变得困难，因此将该目标函数进行简化：使用机器人的指令力F_d代替在基座标系R₀(x₀,y₀,z₀)下的接触力F，则有：Specifically, a large number of nonlinear features are included in formula (12), including the Jacobian matrix, and the real-time contact force

This makes the subsequent controller design difficult, so the objective function is simplified: use the command force F _d of the robot to replace the contact force F under the base frame R ₀ (x ₀ , y ₀ , z ₀ ), then Have:

因此通过求取G₂对θ的梯度，得到其在速度层上的替代描述：When F _d is not related to θ, using F _d can greatly reduce the nonlinearity of the objective function; on the other hand, with a reasonable controller design, the contact force F will eventually converge to F _d , so the objective function before and after replacement is finally Equivalently, the objective function described by formula (13) is defined in the joint angle layer, since the final control quantity is the joint angular velocity

Therefore, by taking the gradient of G2 to θ, its alternative description _on the velocity layer is obtained:

Derivation with respect to J ^T (θ)F _d yields:

是：in,

Yes:

Let H=[H ₁ ,...,H _n ], the above formula can describe the number as:

不相关，则选择最终目标函数选取为

Since the second term in equation (15) is the same as

is not relevant, then the final objective function is selected as

而言是非凸的，通过引入一个修正项

对目标函数中的

进行凸化处理，则最终的约束优化模型为：Since the objective function described by Eq. (21a) is

is non-convex in terms of

in the objective function

After convexization, the final constrained optimization model is:

表示所述机器人的指令力的转秩；J表示Jacobian矩阵；

表示机器人的关节角速度；r_r表示参考指令；A表示简写的矩阵。in,

Represents the rotation rank of the command force of the robot; J represents the Jacobian matrix;

Represents the joint angular velocity of the robot; r _r represents the reference command; A represents the abbreviated matrix.

Update module 26: for updating the state variables and control torques in the final constrained optimization model based on the dynamic neural network model;

为约束公式(22b)和(22c)的对偶状态变量，则拉格朗日函数选取为：In the specific implementation process of the present invention, the input state variable is defined

is the dual state variable of constraint formulas (22b) and (22c), then the Lagrangian function is selected as:

According to the Karush-Kuhn-Tucker condition, optimizing the optimal solution in the constrained optimization model is equivalently expressed as:

where P _Ω ( ) is a clipping function defined as:

为一个投影函数，定义为：

is a projection function, defined as:

When solving equation (24) in real time, the position-force controller design based on the dynamic neural network model is designed as:

表示实数；

表示输入状态量λ₂的一阶导数；

表示输入状态量λ₁的一阶导数；

表示关节角速度；

表示关节角加速度；J表示Jacobian矩阵；r_r表示参考指令；A表示简写矩阵；g₁表示第一个元素；g_2m表示第2m个元素；∈表示一个正常数。in,

represents a real number;

represents the first derivative of the input state quantity λ ₂ ;

represents the first derivative of the input state quantity λ ₁ ;

represents the joint angular velocity;

represents the joint angular acceleration; J represents the Jacobian matrix; r _r represents the reference command; A represents the abbreviated matrix; g ₁ represents the first element; g _2m represents the 2mth element; ∈ represents a positive number.

Judgment module 27: for judging whether the current time is greater than the task time, if so, end the compliance force control of the robot, otherwise, return to obtain the current rotation matrix based on the initialization state variable.

In the specific implementation process of the present invention, it is necessary to judge whether the current time is greater than the task time, and when it is greater than the task time; when it is judged that the current time is greater than the task time, the compliance control of the robot is ended.

In the embodiment of the present invention, through the real-time method of the present invention, high-precision force control in the direction of contact force and motion control in the direction of free motion can be simultaneously realized; online optimization of joint torque can be realized; and in the compliance force control process guarantees that the robot does not exceed its physical constraints.

Those of ordinary skill in the art can understand that all or part of the steps in the various methods of the above embodiments can be completed by instructing relevant hardware through a program, and the program can be stored in a computer-readable storage medium, and the storage medium can include: Read Only Memory (ROM, Read Only Memory), Random Access Memory (RAM, Random Access Memory), magnetic disk or optical disk, etc.

In addition, a method and system for controlling the compliance force of a collaborative robot provided by the embodiments of the present invention have been described in detail above. In this paper, specific examples should be used to illustrate the principles and implementations of the present invention. The descriptions of the above embodiments are only It is used to help understand the method of the present invention and its core idea; at the same time, for those of ordinary skill in the art, according to the idea of the present invention, there will be changes in the specific embodiments and application scope. The contents of the description should not be construed as limiting the present invention.

Claims (4)

1. A method of cooperative robotic compliance control, the method comprising:

acquiring and initializing state variables of the robot;

obtaining a current rotation matrix based on the initialized state variable;

reading current state feedback information of the robot based on the current rotation matrix, wherein the current state feedback informationAnd is comprised in the base coordinate system R₀(x₀,y₀,z₀) The lower contact force, the joint angular velocity of the robot and the joint angle of the robot;

constructing equality constraint for realizing compliance force control and inequality constraint of joint angle, joint angular velocity and joint moment in the robot system based on the current state feedback information;

rewriting a joint moment function, and obtaining a final constraint optimization model;

updating the state variable and the joint moment in the final constraint optimization model based on the dynamic neural network model;

judging whether the current time is greater than the task time, if so, ending the control of the compliance force of the robot, otherwise, returning to obtain the current rotation matrix based on the initialized state variable;

the constructing of the equality constraint for realizing the compliance force control based on the current state feedback information comprises:

feedback information of current state obtained under robot motion modeling system for given expected track x_dContact force F_tThe goal of achieving position-force control is:

then an error vector is defined:

e＝r-r_d＝[F-F_d；e₀]； (16)

the equation is reconstructed in the velocity layer to the form:

wherein k represents a positive control constant;

representing a joint angular velocity of the robot;

representing the first derivative of the error vector;

a first derivative representing the desired trajectory;

is represented by r_dFirst derivative of r_d＝[F_d；0]，F_dRepresenting a commanded force of the robot; f represents in the base coordinate system R₀(x₀,y₀,z₀) A lower contact force; j represents a Jacobian matrix;

the inequality constraint of joint angle, joint angular velocity and joint moment in the robot system includes:

the inequality constraint normalization is described as an inequality constraint for the velocity layer:

wherein,

then the inequality constraint for the joint moment is rewritten as:

wherein, beta>0, then the end effector and the workpiece are aligned in a base coordinate system R₀(x₀,y₀,z₀) When the lower contact force is F, the expression derivation of the moment of action it exerts at each joint can be found:

the joint moment description in the angular velocity layer can be obtained by combining the formulas (18) and (19):

wherein,

j represents a Jacobian matrix;

a first derivative representing a joint moment constraint; τ represents joint moment constraints; β represents a positive control parameter; θ represents a joint angle of the robot;

representing a joint angular velocity of the robot; h represents a real number array;

a first derivative representing a commanded force of the robot;

represents a real number;

before the state variables of the robot are obtained and initialized, the method further comprises the following steps:

respectively modeling in a tool coordinate system and a base coordinate system according to the orthogonal characteristic of the contact force between the robot motion and the workpiece to obtain a robot motion modeling system;

wherein the base coordinate system is represented by R₀(x₀,y₀,z₀) (ii) a Tool coordinate system denoted R_t(x_t,y_t,z_t)；

The machine isContact force between robot and workpiece and z in the tool coordinate system_tParallel, while x_tAnd y_tDefining a free motion of the robotic end effector;

in the operation process of the robot, the actual position x and the expected track x of the robot end effector_dWith a slight deviation in the base coordinate system R₀(x₀,y₀,z₀) And said tool coordinate system R_t(x_t,y_t,z_t) Described below as the deviation deltaX of the robot from the desired trajectory in the base coordinate system and the deviation deltaX of the robot from the desired trajectory in the tool coordinate system, respectively_t；

The process of respectively modeling in the tool coordinate system and the base coordinate system to obtain the robot motion modeling system is as follows:

in the tool coordinate system R_t(x_t,y_t,z_t) As a consequence of neglecting the friction between the robot and the workpiece, assuming that the contact between the robot and the workpiece is a rigid contact, the contact force is described as:

F_t＝k_f∑_tδX_t； (1)

wherein k is_fRepresenting a stiffness coefficient; sigma_tA diagonal matrix is used to describe the deviation δ X of the robot from the desired trajectory in the tool coordinate system_tThe relationship with its contact force, where 0 means that displacement in that direction does not produce a contact force, and conversely 1;

definition of

Then in said tool coordinate system R_t(x_t,y_t,z_t) Lower position tracking error e_tComprises the following steps:

using a rotation matrix S with a known contact surface_tDescribing the tool coordinate system R_t(x_t,y_t,z_t) And the basic coordinate system R₀(x₀,y₀,z₀) The rotational relationship between them;

definitions F and e₀Respectively being said base coordinate system R₀(x₀,y₀,z₀) Lower F_tAnd e_tThe corresponding description of (1) then includes:

e_t＝S_te₀； (4)

δX_t＝S_tδX； (5)

by the simultaneous expression of formulas (1) to (5), there are:

wherein δ X represents the robot in the base coordinate system R₀(x₀,y₀,z₀) (iv) deviation from the desired trajectory; f represents in the base coordinate system R₀(x₀,y₀,z₀) A lower contact force;

in the base coordinate system R₀(x₀,y₀,z₀) The medium displacement δ X is described as δ X ═ X-X_dWherein the desired trajectory x_dIs a base coordinate system R₀(x₀,y₀,z₀) The desired position signal described in (1), and therefore, equations (6) - (7) are rewritten as:

defining the expected track and the command force of the robot as x respectively_dAnd F_d(ii) a Then, according to the descriptions of the formula (5) and the formula (9), the control target is described as designing a position-force control strategy facing the redundant robot, such that the contact force F → F described by the formula (8)_dWhile making the tracking error e described by equation (9)₀→0；∑_tRepresenting a parameter matrix describing the contact;

representing a parameter matrix describing the motion;

in the robot motion modeling system, for simplifying the description, definition is carried out

r_d＝[F_d；0]，

Then formula (8) and formula (9) are rewritten as:

A(f(θ)-x_d)＝r； (10)

the control objective is described by designing the joint such that r is r_d；∑_tRepresenting a parameter matrix describing the contact;

representing a parameter matrix describing the motion.

2. The cooperative robotic compliance control method of claim 1, wherein the adapting the joint moment function and obtaining a final constraint optimization model comprises:

simplifying the objective function by using the command force F of the robot_dInstead of in the base coordinate system R₀(x₀,y₀,z₀) The following contact forces F, then:

if the objective function described in equation (13) is defined in the joint angle layer, the final control quantity is the joint angular velocity

Thus by finding G₂For the gradient of θ, an alternative description of it on the velocity layer is obtained:

to J^T(θ)F_dThe derivation yields:

wherein,

the method comprises the following steps:

let H ═ H₁,…,H_n]Then the above formula can describe the number as:

due to the second term in equation (15)

Not correlation, then choose the final objective function to choose as

By introducing a correction term

For in the objective function

And (4) carrying out convex processing, wherein the final constraint optimization model is as follows:

wherein,

a rank representing a commanded force of the robot; j represents a Jacobian matrix;

representing a joint angular velocity of the robot; r is_rRepresents a reference instruction; a denotes a matrix of abbreviations.

3. The cooperative robotic compliance force control method of claim 1, wherein the updating state variables and joint moments in the final constraint optimization model based on the dynamic neural network model comprises:

defining input state variables

For dual state variables in the constraint optimization model, the lagrangian function is selected as follows:

according to the Karush-Kuhn-Tucker condition, optimizing the optimal solution in the constraint optimization model is equivalently expressed as:

wherein, P_Ω(. cndot.) is a clipping function defined as:

is a projection function defined as:

in solving equation (24) in real time, the position-force controller based on the dynamic neural network model is designed as:

wherein,

represents a real number;

representing an input state variable lambda₂The first derivative of (a);

representing an input state variable lambda₁The first derivative of (a);

represents the joint angular velocity;

represents the joint angular acceleration; j represents a Jacobian matrix; r is_rRepresents a reference instruction; a represents a shorthand matrix; g₁Represents the first element; g_2mRepresents the 2 m-th element; e represents a positive constant.

4. A cooperative robotic compliance control system, the system comprising:

an initialization module: the robot state variable acquiring and initializing system is used for acquiring state variables of the robot and initializing the state variables;

an obtaining module: the device comprises a processor, a processor and a controller, wherein the processor is used for obtaining a current rotation matrix based on an initialization state variable;

a reading module: for reading current state feedback information of the robot based on the current rotation matrix, the current state feedback information being included in a base coordinate system R₀(x₀,y₀,z₀) The lower contact force, the joint angular velocity of the robot and the joint angle of the robot;

constructing a module: the system comprises a robot system, a controller and a controller, wherein the robot system is used for establishing equality constraint for realizing compliance force control and inequality constraint of joint angle, joint angular velocity and joint moment in the robot system based on the current state feedback information;

and a rewriting module: the joint moment function is rewritten, and a final constraint optimization model is obtained;

an update module: updating state variables and joint moments in the final constraint optimization model based on the dynamic neural network model;

a judging module: the system is used for judging whether the current time is greater than the task time, if so, ending the control of the compliance force of the robot, otherwise, returning to obtain the current rotation matrix based on the initialized state variable;

the constructing of the equality constraint for realizing the compliance force control based on the current state feedback information comprises:

feedback information of current state obtained under robot motion modeling system for given expected track x_dContact force F_tThe goal of achieving position-force control is:

then an error vector is defined:

e＝r-r_d＝[F-F_d；e₀]； (16)

the equation is reconstructed in the velocity layer to the form:

wherein k represents a positive control constant;

representing a joint angular velocity of the robot;

representing the first derivative of the error vector;

a first derivative representing the desired trajectory;

is represented by r_dFirst derivative of r_d＝[F_d；0]，F_dRepresenting a commanded force of the robot; f represents in the base coordinate system R₀(x₀,y₀,z₀) A lower contact force; j represents a Jacobian matrix;

the inequality constraint of joint angle, joint angular velocity and joint moment in the robot system includes:

the inequality constraint normalization is described as an inequality constraint for the velocity layer:

wherein,

then the inequality constraint for the joint moment is rewritten as:

wherein, beta>0, then the end effector and the workpiece are aligned in a base coordinate system R₀(x₀,y₀,z₀) When the lower contact force is F, the expression derivation of the moment of action it exerts at each joint can be found:

the joint moment description in the angular velocity layer can be obtained by combining the formulas (18) and (19):

wherein,

j represents a Jacobian matrix;

a first derivative representing a joint moment constraint; τ represents joint moment constraints; β represents a positive control parameter; θ represents a joint angle of the robot;

representing a joint angular velocity of the robot; h represents a real number array;

a first derivative representing a commanded force of the robot;

represents a real number;

before the state variables of the robot are obtained and initialized, the method further comprises the following steps:

respectively modeling in a tool coordinate system and a base coordinate system according to the orthogonal characteristic of the contact force between the robot motion and the workpiece to obtain a robot motion modeling system;

wherein the base coordinate system is represented by R₀(x₀,y₀,z₀) (ii) a Tool coordinate system denoted R_t(x_t,y_t,z_t)；

Contact force between the robot and workpiece and z in the tool coordinate system_tParallel, while x_tAnd y_tDefining a free motion of the robotic end effector;

in the operation process of the robot, the actual position x and the expected track x of the robot end effector_dWith a slight deviation in the base coordinate system R₀(x₀,y₀,z₀) And said tool coordinate system R_t(x_t,y_t,z_t) Described below as the deviation deltaX of the robot from the desired trajectory in the base coordinate system and the deviation deltaX of the robot from the desired trajectory in the tool coordinate system, respectively_t；

The process of respectively modeling in the tool coordinate system and the base coordinate system to obtain the robot motion modeling system is as follows:

in the tool coordinate system R_t(x_t,y_t,z_t) As a consequence of neglecting the friction between the robot and the workpiece, assuming that the contact between the robot and the workpiece is a rigid contact, the contact force is described as:

F_t＝k_f∑_tδX_t； (1)

wherein k is_fRepresenting a stiffness coefficient; sigma_tA diagonal matrix is used to describe the deviation δ X of the robot from the desired trajectory in the tool coordinate system_tThe relationship with its contact force, where 0 means that displacement in that direction does not produce a contact force, and conversely 1;

definition of

Then in said tool coordinate system R_t(x_t,y_t,z_t) Lower position tracking error e_tComprises the following steps:

using a rotation matrix S with a known contact surface_tDescribing the tool coordinate system R_t(x_t,y_t,z_t) And the basic coordinate system R₀(x₀,y₀,z₀) The rotational relationship between them;

definitions F and e₀Respectively being said base coordinate system R₀(x₀,y₀,z₀) Lower F_tAnd e_tThe corresponding description of (1) then includes:

e_t＝S_te₀； (4)

δX_t＝S_tδX； (5)

by the simultaneous expression of formulas (1) to (5), there are:

wherein δ X represents the robot in the base coordinate system R₀(x₀,y₀,z₀) (iv) deviation from the desired trajectory; f represents in the base coordinate system R₀(x₀,y₀,z₀) A lower contact force;

in the base coordinate system R₀(x₀,y₀,z₀) The medium displacement δ X is described as δ X ═ X-X_dWherein the desired trajectory x_dIs a base coordinate system R₀(x₀,y₀,z₀) The desired position signal described in (1), and therefore, equations (6) - (7) are rewritten as:

defining the expected track and the command force of the robot as x respectively_dAnd F_d(ii) a Then, according to the descriptions of the formula (5) and the formula (9), the control target is described as designing a position-force control strategy facing the redundant robot, such that the contact force F → F described by the formula (8)_dWhile making the tracking error e described by equation (9)₀→0；∑_tRepresenting a parameter matrix describing the contact;

representing a parameter matrix describing the motion;

in the robot motion modeling system, for simplifying the description, definition is carried out

r_d＝[F_d；0]，

Then formula (8) and formula (9) are rewritten as:

A(f(θ)-x_d)＝r； (10)

the control objective is described by designing the joint such that r is r_d；∑_tRepresenting a parameter matrix describing the contact;

representing a parameter matrix describing the motion.

Images