A robot force control method and device, a robot and a storage medium

Abstract

The application relates to a robot force control method, device, robot and storage medium, the robot force control method comprises the following steps: acquiring the contact force and torque of a tool carried by a robot and an environment in a tool coordinate system; controlling the contact force and torque in the tool coordinate system by using a mobility control and an integral control to obtain a position compensation amount in the tool coordinate system; converting the position compensation amount in the tool coordinate system into a position compensation amount in a joint space to obtain a joint compensation amount, and synchronously controlling the position and force of the tool carried by the robot according to the joint compensation amount. The robot force control method provided by the application improves the responsiveness, stability and steady-state accuracy of robot force control.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of robots, in particular to a robot force control method and device, a robot and a storage medium. BACKGROUND

[0002] In many applications of industrial robots, polishing and grinding are an indispensable basic process. In addition, since the grinding dust is harmful to the human body, robots have become an irreversible trend to replace people for grinding work. As one of the key technologies for robot grinding applications, robot force control function is particularly important. Therefore, how to design and implement a force control function with fast response, high stability and high steady-state accuracy based on the existing industrial robot architecture is also a problem that needs to be solved in the robot grinding industry. SUMMARY

[0003] Therefore, the present application provides a robot force control method, comprising the steps of:

[0004] Obtaining the contact force and torque of the tool carried by the robot and the environment in the tool coordinate system;

[0005] Using the admittance control and integral control algorithm, the position compensation amount in the tool coordinate system is obtained according to the contact force and torque in the tool coordinate system;

[0006] The position compensation amount in the tool coordinate system is converted into joint compensation amount in the joint space;

[0007] The joint of the robot is controlled according to the joint compensation amount.

[0008] Optionally, the contact force and torque of the tool carried by the robot and the environment in the tool coordinate system comprises:

[0009] Obtaining the rotation matrix of the robot base coordinate system to the force sensor and the gravity acceleration vector in the robot base coordinate system; obtaining the statics parameters of the tool carried by the robot and the static bias of the force / torque sensor according to the rotation matrix of the robot base coordinate system to the force sensor and the gravity acceleration vector in the robot base coordinate system; obtaining the contact force and torque of the tool carried by the robot and the environment in the tool coordinate system according to the statics parameters of the tool carried by the robot and the static bias of the force / torque sensor; wherein the statics parameters of the tool carried by the robot include tool mass and first order mass moment.

[0010] Optionally, the statics parameters of the tool carried by the robot and the static bias of the force / torque sensor are obtained according to the rotation matrix of the robot base coordinate system to the force sensor and the gravity acceleration vector in the robot base coordinate system, comprising:

[0011] According to the rotation matrix of the robot base coordinate system to the force sensor, the gravity acceleration vector under the robot base coordinate system, and a parameter calculation formula, static parameters of a tool carried by the robot and static bias of the force / torque sensor are obtained, and the parameter calculation formula is

[0012] [m mr T f0 T ] T =(A T A) -1 A T F

[0013] wherein m is the tool mass, mr is the first-order mass moment of the tool, f0 is the static bias of the force / torque sensor, A = [A1 T A2 T …A N T ] T , A i is the [R F 0 g0 0 3*3 I3 0 3*3 ;0 3*1 –S(R F 0 g0) 0 3*3 I3] corresponding to the i-th sampling point, R F 0 is the rotation matrix of the robot base coordinate system to the force sensor, g0 is the gravity acceleration vector under the robot base coordinate system, S is the cross multiplication operator, 0 3*3 is a third-order zero matrix, I3 is a third-order unit matrix, F = [f1 T ...f2 T ...f N T ] T , f i is the measurement data of the force and torque corresponding to the i-th sampling point

[0014] Optionally, the obtaining of the contact force and torque between the tool carried by the robot and the environment under the tool coordinate system according to the static parameters of the tool carried by the robot and the static bias of the force / torque sensor comprises:

[0015] the static parameters of the tool carried by the robot and the static bias of the force / torque sensor are taken as tool compensation values, force / torque sensor data is obtained, the force / torque sensor data is subtracted by the tool compensation values, the contact force and torque between the tool carried by the robot and the environment under the sensor coordinate system are obtained, the contact force and torque between the tool carried by the robot and the environment under the sensor coordinate system are coordinate-converted to obtain the contact force and torque between the tool carried by the robot and the environment under the tool coordinate system.

[0016] Optionally, the contact force and moment of the robot-carrying tool and the environment in the sensor coordinate system are coordinate-transformed to obtain the contact force and moment of the robot-carrying tool and the environment in the tool coordinate system, comprising:

[0017] The contact force and moment of the robot-carrying tool and the environment in the sensor coordinate system are coordinate-transformed by using a coordinate transformation relationship to obtain the contact force and moment of the robot-carrying tool and the environment in the tool coordinate system, and the coordinate transformation relationship is

[0018] f F c = f - [R F 0 g0 0 3*3 I3 0 3*3 ;0 3*1 –S(R F 0 g0) 0 3*3 I3]·[m mr T f0 T ] T

[0019] f T c = H T F f F c

[0020] H T F = [R T F 0 3*3 ;S(p TF ) R T F R T F ]

[0021] wherein f F c is the contact force and moment in the sensor coordinate system, f T c is the contact force and moment in the tool coordinate system, H T F is a transfer matrix from the sensor coordinate system to the tool coordinate system, R T F is a rotation matrix from the sensor coordinate system to the tool coordinate system, and p TF is a vector from the origin of the sensor coordinate system to the origin of the tool coordinate system in the tool coordinate system.

[0022] Optionally, the formula of the admittance control and integral control algorithm is:

[0023] Δx = (K + Bs) -1 ·(f tartet -f F c )+K i ∫(f tartet -f F c ); wherein Δx is a position compensation in a tool coordinate system, K and B are a stiffness matrix and a damping matrix in the tool coordinate system respectively, s is a Laplace operator, K i is an integral gain matrix, and f tartet is a target force and torque in the tool coordinate system.

[0024] Optionally, the position compensation in the tool coordinate system is converted into a joint space position compensation to obtain a joint compensation, comprising:

[0025] The position compensation in the tool coordinate system is converted into a joint space joint compensation by using a compensation conversion relationship; wherein the compensation conversion relationship is Δq = sat(J -1 Δx,q u ,q l ), Δq is the joint compensation, J is a Jacobian matrix in the tool coordinate system, q u is an upper limit of the joint compensation, q l is a lower limit of the joint compensation, and Δx is the position compensation in the tool coordinate system.

[0026] Optionally, the joint of the robot is controlled according to the joint compensation, comprising:

[0027] The joint compensation is filtered by using a tracking differentiator, and a filtered joint compensation is outputted; the filtered joint compensation is superimposed into a planned motion instruction to obtain a superimposed motion instruction, and the joint of the robot is controlled by using the superimposed motion instruction.

[0028] Optionally, the description formula of the tracking differentiator is

[0029]

[0030] wherein T s is a sampling time, r s - is a joint position compensation at a previous sampling time, r s + is a joint position compensation at a current sampling time, is a derivative of the joint position compensation at the previous sampling time, is the derivative of joint position compensation for the current sampling time, A max is the upper bound of the second derivative of joint position compensation, f is the second derivative of joint position compensation, V max is the upper bound of the derivative of joint position compensation, r in is the input joint position compensation, sat is a saturation function, and fhan is a nonlinear function.

[0031] The application further provides a robot force control device, comprising a contact force and torque acquisition module, a position compensation amount acquisition module, a joint compensation amount acquisition module, and a force control module.

[0032] The contact force and torque acquisition module is configured to acquire the contact force and torque between a tool carried by the robot in the tool coordinate system and the environment.

[0033] The position compensation amount acquisition module is configured to acquire the position compensation amount in the tool coordinate system by using the admittance control and integral control algorithms according to the contact force and torque in the tool coordinate system.

[0034] The joint compensation amount acquisition module is configured to convert the position compensation amount in the tool coordinate system into the joint compensation amount in the joint space.

[0035] The force control module is configured to control the joint of the robot according to the joint compensation amount.

[0036] The application further provides a robot, comprising a processor and a memory coupled to the processor, wherein the memory stores program instructions executable by the processor; and the processor implements the robot force control method according to any of the above technical solutions when executing the program instructions stored in the memory.

[0037] The application further provides a storage medium, wherein the storage medium stores program instructions, and the program instructions are executable by a processor to implement the robot force control method according to any of the above technical solutions.

[0038] The robot force control method, device, robot, and storage medium provided by the application acquire the contact force and torque between a tool carried by the robot in the tool coordinate system and the environment; acquire the position compensation amount in the tool coordinate system by using the admittance control and integral control algorithms according to the contact force and torque in the tool coordinate system; convert the position compensation amount in the tool coordinate system into the joint compensation amount in the joint space; and control the joint of the robot according to the joint compensation amount. The admittance control is used to ensure the responsiveness and stability of force control, and the integral control is used to ensure the steady-state accuracy of force control, thereby improving the responsiveness, stability, and steady-state accuracy of robot force control. BRIEF DESCRIPTION OF DRAWINGS

[0039] Figure 1 A flowchart of a robot force control method provided by an embodiment of the present application is shown in FIG. 1.

[0040] Figure 2 A contact force and torque curve diagram in a tool coordinate system provided by an embodiment of the present application is shown in FIG. 4.

[0041] Figure 3 A position compensation amount diagram provided by an embodiment of the present application is shown in FIG. 5.

[0042] Figure 4 A final position compensation amount diagram provided by an embodiment of the present application is shown in FIG. 6.

[0043] Figure 5 A structural diagram of a robot force control device provided by an embodiment of the present application is shown in FIG. 7.

[0044] Figure 6 A structural diagram of a storage medium provided by an embodiment of the present application is shown in FIG. 8.

[0045] The meanings of the respective reference numerals in the drawings are as follows:

[0046] 50 - robot force control device; 51 - contact force and torque acquisition module; 52 - position compensation amount acquisition module; 53 - force control module; 60 - storage medium; 61 - program instruction. DETAILED DESCRIPTION

[0047] In order to facilitate the understanding of the present application, the present application will be described more fully below with reference to the accompanying drawings. The preferred embodiments of the present application are shown in the drawings. However, the present application can be realized in many different forms and is not limited to the embodiments described herein. Rather, these embodiments are provided so that the disclosure of the present application can be more thoroughly and completely understood.

[0048] It should be noted that when an element is referred to as being "fixed" to another element, it can be directly on the other element or there can be an intervening element present. When an element is referred to as being "connected" to another element, it can be directly connected to the other element or intervening elements can be present.

[0049] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which the present application belongs. The terminology used in the description of the present application herein is for the purpose of describing particular embodiments only and is not intended to be limiting of the present application.

[0050] Figure 1 A flowchart of a robot force control method provided by an embodiment of the present application is shown in FIG. 1. Figure 1 The flow order shown in FIG. 1 is not limiting. For example, the steps can be performed in a different order. Figure 1As shown, the robot force control method mainly includes the following steps S101-S104:

[0051] S101, acquiring the contact force and torque between the tool carried by the robot and the environment in the tool coordinate system;

[0052] Wherein, before force control, the statics parameters of the tool (the tool carried by the robot, such as a polishing head) mounted on the robot are calibrated by using the force / torque sensor, the statics parameters including mass, first order mass moment, and the static bias of the force / torque sensor (under no load) is acquired, and the statics parameters and the static bias are taken as the position compensation value in the sensor coordinate; in the force control stage, the data collected by the force / torque sensor is acquired, the position compensation value in the sensor coordinate is subtracted to obtain the contact force and torque between the tool and the environment in the sensor coordinate system, and then the contact force and torque between the tool carried by the robot and the environment in the tool coordinate system are acquired by using coordinate conversion. The force / torque sensor can be a six-dimensional force / torque sensor, which can measure three forces and three torques.

[0053] S102, acquiring the position compensation amount in the tool coordinate system according to the contact force and torque in the tool coordinate system by using the admittance control and integral control algorithm;

[0054] Wherein, the admittance control is used to adjust the responsiveness and stability, and the integral control is used to ensure the steady state accuracy.

[0055] S103, converting the position compensation amount in the tool coordinate system into the position compensation amount in the joint space to obtain the joint compensation amount;

[0056] Wherein, the position compensation amount in the tool coordinate system is the position adjustment amount in the Euclidean space, in order to control the robot joint, it needs to be converted into the position compensation amount in the joint space to obtain the joint compensation amount.

[0057] S104, controlling the joint of the robot according to the joint compensation amount.

[0058] The robot force control method provided by the embodiment of the application acquires the contact force and torque between the tool carried by the robot and the environment in the tool coordinate system; acquires the position compensation amount in the tool coordinate system according to the contact force and torque in the tool coordinate system by using the admittance control and integral control algorithm; converts the position compensation amount in the tool coordinate system into the joint compensation amount in the joint space; controls the joint of the robot according to the joint compensation amount; the responsiveness and stability of the force control are ensured by the admittance control, and the steady state accuracy of the force control is ensured by the integral control, so that the responsiveness, stability and steady state accuracy of the robot force control are improved.

[0059] In an optional embodiment, step S101 specifically includes the following steps:

[0060] S201, obtaining a rotation matrix of a robot base coordinate system to a force sensor and a gravity acceleration vector in the robot base coordinate system;

[0061] The rotation matrix of the robot base coordinate system to the force sensor can be obtained by bringing the joint angle into the homogeneous transformation matrix, and the gravity acceleration vector in the robot base coordinate system is usually [0 0 -9.81] T .

[0062] S202, obtaining statics parameters of a tool carried by the robot and static bias of the force / torque sensor according to the rotation matrix of the robot base coordinate system to the force sensor and the gravity acceleration vector in the robot base coordinate system;

[0063] In order to obtain the statics parameters and the static bias of the force / torque sensor, the robot carries the tool to hover at different positions, and collects force / torque sensor data and robot joint encoder data, and then calculates the statics parameters of the tool by using the least square method.

[0064] In one embodiment, the robot kinematics parameters used are shown in Table 1.

[0065] Table 1 Robot kinematics parameters

[0066]

[0067] In Table 1, link number is joint number, a is the distance of two joint axes along the common axis, α is the included angle between two joint axes, d is the joint translation distance, and θ is the joint translation angle.

[0068] The collected robot joint encoder data is shown in Table 2.

[0069] Table 2 Robot joint encoder data

[0070]

[0071] Table 2 includes four groups of data, each group of data including six joint encoder data (radian).

[0072] The collected force / torque sensor data is shown in Table 3.

[0073] Table 3 Force / torque sensor data

[0074]

[0075] Table 3 includes four groups of data, each group of data including three directions of force and three directions of torque.

[0076] S203, obtaining the contact force and torque between the tool carried by the robot and the environment in the tool coordinate system according to the statics parameters of the tool carried by the robot and the static bias of the force / torque sensor; wherein the statics parameters of the tool carried by the robot include tool mass and first-order mass moment.

[0077] In the force control stage, the force / torque sensor data is obtained, the position compensation value in the sensor coordinate is subtracted, i.e. the statics parameters and the static bias of the force / torque sensor are subtracted, to obtain the contact force and torque between the tool and the environment in the sensor coordinate system, and then the contact force and torque between the tool carried by the robot and the environment in the tool coordinate system are obtained by using coordinate conversion.

[0078] In an optional embodiment, step S202 specifically includes the following steps:

[0079] According to the rotation matrix of the robot base coordinate system to the force sensor, the gravity acceleration vector in the robot base coordinate system and the parameter calculation formula, the statics parameters of the tool carried by the robot and the static bias of the force / torque sensor are obtained, and the parameter calculation formula is

[0080] [m mr T f0 T ] T =(A T A) -1 A T F

[0081] Wherein m is the tool mass, mr is the first-order mass moment of the tool, f0 is the static bias of the force / torque sensor, A=[A1 T A2 T …A N T ] T , A i is the [R F 0 g0 0 3*3 I3 0 3*3 ;0 3*1 –S(R F 0 g0) 0 3*3 I3] corresponding to the i-th sampling point, R F 0 is the rotation matrix of the robot base coordinate system to the force sensor, g0 is the gravity acceleration vector in the robot base coordinate system, S is the cross multiplication operator, 0 3*3 is a third-order zero matrix, I3 is a third-order unit matrix, F=[f1 T ...f2 T ...f N T ] T , fi The measured data of force and torque corresponding to the i-th sampling point.

[0082] Wherein, the robot is made to hover at different positions with tools, and force / torque sensor data is sampled. For a certain sampling point, the following formula is used

[0083] [R F 0 g0 0 3*3 I3 0 3*3 ;0 3*1 –S(R F 0 g0) 0 3*3 I3]·[m mr T f0 T ] T =f

[0084] In the above formula, R F 0 is the rotation matrix of the robot base coordinate system to the force sensor, g0 is the gravity acceleration vector in the robot base coordinate system, S is the cross multiplication operator, that is, the cross multiplication of the left vector is expanded into a three-order square matrix, f is the measured data of force and torque at the sampling point, I3 is a three-order unit matrix, wherein [m mr T f0 T ] T are to be identified parameters, and other parameters can be calculated or collected.

[0085] Randomly sample N sampling points, and have

[0086] A·[m mr T f0 T ] T =F

[0087] The statics parameters are calculated by the least square method, that is:

[0088] [m mr T f0 T ] T =(A T A) -1 A T F

[0089] After identifying the parameters m, mr and f0, the tool load can be compensated online according to the joint angle. In a specific embodiment, the statics parameters are obtained, as shown in Table 4.

[0090] Table 4 Statics parameters

[0091]

[0092] Column 1 in Table 4 is mass, columns 2-4 are first order mass moments in three directions, columns 5-7 are static biases of forces in three directions, and columns 8-10 are static biases of moments of forces in three directions.

[0093] In an optional embodiment, the contact force and moment between the tool carried by the robot and the environment in the tool coordinate system are obtained according to the statics parameters of the tool carried by the robot and the static biases of the force / torque sensor, and specifically include:

[0094] The force / torque sensor data is obtained by taking the statics parameters of the tool carried by the robot and the static biases of the force / torque sensor as tool compensation values, the contact force and moment between the tool carried by the robot and the environment in the sensor coordinate system are obtained by subtracting the tool compensation values from the force / torque sensor data, the contact force and moment between the tool carried by the robot and the environment in the sensor coordinate system are coordinate-converted to obtain the contact force and moment between the tool carried by the robot and the environment in the tool coordinate system.

[0095] In the force control stage, the force / torque sensor data is collected, and the tool compensation values are subtracted to obtain the contact force and moment between the tool and the environment. Since the contact force and moment information is measured in the sensor coordinate system, it needs to be converted to the tool coordinate system for control.

[0096] In an optional embodiment, the contact force and moment between the tool carried by the robot and the environment in the sensor coordinate system are coordinate-converted to obtain the contact force and moment between the tool carried by the robot and the environment in the tool coordinate system, and specifically include:

[0097] The contact force and moment between the tool carried by the robot and the environment in the sensor coordinate system are coordinate-converted by using a coordinate conversion relationship to obtain the contact force and moment between the tool carried by the robot and the environment in the tool coordinate system, and the coordinate conversion relationship is:

[0098] f F c = f - [R F 0 g0 0 3*3 I3 0 3*3 ;0 3*1 –S(R F 0 g0) 0 3*3 I3]·[m mr T f0 T ] T

[0099] f T c = H T F f Fc

[0100] H T F =[R T F 0 3*3 ;S(p TF )R T F R T F ]

[0101] wherein f F c is the contact force and moment in the sensor coordinate system (contact force and moment between the tool carried by the robot and the environment in the sensor coordinate system), f T c is the contact force and moment in the tool coordinate system (contact force and moment between the tool carried by the robot and the environment in the tool coordinate system), H T F is the transfer matrix from the sensor coordinate system to the tool coordinate system, R T F is the rotation matrix from the sensor coordinate system to the tool coordinate system, p TF is the vector from the origin of the sensor coordinate system to the origin of the tool coordinate system in the tool coordinate system.

[0102] In one embodiment, the joint position collected at the current time is q = [0.0 0.3255 0.4125 0.01.2345 0.0], the force and moment information collected is f = [4.7464 0.2355 14.3784 0.0242 -0.3877 -0.0152], according to the identified tool statics parameters and the calculated contact force and moment f F c = [-0.0200 0.0010 4.990 0.0010 0.0020 0.0000] in the force and moment coordinate system (sensor coordinate system), the contact force and moment between the tool carried by the robot and the environment in the tool coordinate system is calculated.

[0103] In the implementation, the tool coordinate system and the force and moment coordinate system coincide, the transfer matrix H T F from the force and moment coordinate system to the tool coordinate system is a unit matrix, thus the calculated contact force and moment f T c in the tool coordinate system is the same as the calculated contact force and moment f F c in the force and moment coordinate system; the target force and moment f target= [0 0 2 0 0 0], the stiffness matrix K and the damping matrix B are both diagonal matrices, and their diagonal elements are [500 500 500 50 50 50] and [10 10 10 1 1 1] respectively, the integral gain matrix is also a diagonal matrix, and its diagonal elements are [0.01 0.01 0.01 0.001 0.001 0.001], the calculated contact force and torque in the tool coordinate system, and a contact force and torque curve diagram in the tool coordinate system are shown in FIG. 1. Figure 2

[0104] In an optional embodiment, the formula of the admittance control and integral control algorithm is as follows:

[0105] Δx = (K + Bs) -1 · (f tartet -f F c ) + K i ∫ (f tartet -f F c ); wherein Δx is the position compensation in the tool coordinate system, K and B are the stiffness matrix and the damping matrix in the tool coordinate system respectively, s is the Laplace operator, K i is the integral gain matrix, and f tartet is the target force and torque in the tool coordinate system.

[0106] It should be noted that the difference between the contact force and torque in the tool coordinate system and the set (target) contact force and torque will be converted into the position compensation in the tool coordinate system, and a position compensation diagram is shown in FIG. 2. Figure 3

[0107] In an optional embodiment, the position compensation in the tool coordinate system is converted into the joint compensation in the joint space, comprising:

[0108] The position compensation in the tool coordinate system is converted into the joint compensation in the joint space by using a compensation conversion relationship; wherein the compensation conversion relationship is Δq = sat(J -1 Δx, q u , q l ), Δq is the joint compensation, J is the Jacobian matrix in the tool coordinate system, q u is the upper limit of the joint compensation, q l is the lower limit of the joint compensation, and Δx is the position compensation in the tool coordinate system.

[0109] ​​wherein the position adjustment amount in the Euclidean space, i.e. the position compensation amount in the tool coordinate system, is converted into the joint compensation amount in the joint space by using the Jacobian matrix of the robot, and the joint compensation amount in the joint space is the joint compensation amount in the joint coordinate system; in order to prevent an excessively large compensation amount in the joint space, the joint compensation amount is limited. In a specific implementation, the upper limit of the joint compensation amount is set as q u =[0.1 0.1 0.1 0.1 0.1 0.1], and the lower limit of the joint compensation amount is set as q l =-[0.1 0.1 0.1 0.1 0.1 0.1].

[0110] In an optional implementation, the joint of the robot is controlled according to the joint compensation amount, comprising:

[0111] The joint compensation amount is filtered by using a tracking differentiator, and the filtered joint compensation amount is outputted; the filtered joint compensation amount is superimposed into the planned motion instruction to obtain a superimposed motion instruction, and the joint of the robot is controlled by using the superimposed motion instruction.

[0112] The joint compensation amount is filtered by using a tracking differentiator, and the filtered joint compensation amount is outputted; the filtered joint compensation amount is superimposed into the planned motion instruction to obtain a superimposed motion instruction, and the joint of the robot is controlled by using the superimposed motion instruction.

[0113] In an optional implementation, the description formula of the tracking differentiator is

[0114]

[0115] wherein T s is a sampling time, r s - is a joint position compensation amount at a previous sampling time, r s + is a joint position compensation amount at a current sampling time, is a derivative of the joint position compensation amount at the previous sampling time, is a derivative of the joint position compensation amount at the current sampling time, A max is an upper limit of a second-order derivative of the joint position compensation amount, f is the second-order derivative of the joint position compensation amount, V max is an upper limit of the derivative of the joint position compensation amount, r in is an input joint position compensation amount, sat is a saturation function, and fhan is a nonlinear function.

[0116] Wherein, the joint position compensation amount is acquired and taken as an input of a tracking differentiator, and an output of the tracking differentiator is an actual joint position compensation amount, and fhan has the following definition,

[0117]

[0118] Finally, the joint position compensation amount output by the tracking differentiator is superimposed on the planned motion instruction to realize synchronous control of position and force. The upper bound A of the second derivative of the joint position compensation amount of the tracking differentiator of each axis max The upper bound V of the first derivative of the joint position compensation amount of the tracking differentiator of each axis can be set to 1. max The sampling time T is set to 4. s The final position compensation amount is 0.002, and a schematic diagram of the final position compensation amount is shown in Figure 4 As shown in Figure 4 It can be known that force control can be directly performed in the position loop to realize hybrid control of force and position, control parameters can be quickly adjusted, and productization of the robot force control function is facilitated.

[0119] Figure 5 A structure schematic diagram of a robot force control device of a second embodiment of the present application is shown in Figure 5 The robot force control device 50 includes a contact force and torque acquisition module 51, a position compensation amount acquisition module 52, a joint compensation amount acquisition module 53, and a force control module 54. The contact force and torque acquisition module 51 is configured to acquire contact force and torque of a tool carried by a robot in a tool coordinate system. The position compensation amount acquisition module 52 is configured to acquire a position compensation amount in the tool coordinate system by using a mobility control algorithm and an integral control algorithm according to the contact force and torque in the tool coordinate system. The joint compensation amount acquisition module 53 is configured to convert the position compensation amount in the tool coordinate system into a joint compensation amount in a joint space. The force control module is configured to control a joint of the robot according to the joint compensation amount.

[0120] The third embodiment of the present application provides a robot including a processor and a memory coupled to the processor, the memory storing program instructions executable by the processor; and the processor executes the program instructions stored in the memory to implement the robot force control method of any one of the above embodiments.

[0121] Figure 6For a structural schematic diagram of the storage medium of the fourth embodiment of the present application, the storage medium 60 of the embodiment of the present application stores program instructions 61, and the program instructions 61 are executed by a processor to implement the robot force control method described in any of the above embodiments. The storage medium can be non-volatile or volatile. The program instructions 61 can be stored in the above storage medium in the form of a software product, and the foregoing storage medium includes: a U disk, a mobile hard disk, a read-only memory (ROM, Read-Only Memory), a random access memory (RAM, Random Access Memory), a magnetic disk or an optical disk, and various media that can store program codes.

[0122] The technical features of the above embodiments can be combined in any manner. In order to make the description simple, all possible combinations of the technical features in the above embodiments are not described, but as long as the combinations of the technical features do not contradict, they should be considered as the scope of the present application.

[0123] The above embodiments only express the preferred implementation manners of the present application, and the description is more specific and detailed, but it should not be understood as a limitation on the scope of the patent of the present application. It should be pointed out that for those skilled in the art, some modifications and improvements can be made without departing from the concept of the present application, and these all belong to the protection scope of the present application. Therefore, the protection scope of the patent of the present application should be subject to the appended claims.

Claims

1. A robot force control method, characterized in that, Including the following steps: The method involves obtaining the contact force and torque between the robot-carried tool and the environment in the tool coordinate system. This includes obtaining the rotation matrix from the robot's base coordinate system to the force sensor and the gravitational acceleration vector in the robot's base coordinate system; obtaining the static parameters of the robot-carried tool and the static offset of the force / torque sensor based on the rotation matrix from the robot's base coordinate system to the force sensor and the gravitational acceleration vector in the robot's base coordinate system; and obtaining the contact force and torque between the robot-carried tool and the environment in the tool coordinate system based on the static parameters of the robot-carried tool and the static offset of the force / torque sensor. The static parameters of the robot-carried tool include the tool mass and the first-order mass moment. Using admittance control and integral control algorithms, the position compensation amount in the tool coordinate system is obtained based on the contact force and torque in the tool coordinate system. The formulas for the admittance control and integral control algorithms are as follows: ∆ x =(K+B s ) -1 ∙( f tartet - f F c )+K i ∫( f tartet - f F c ), where ∆ x K represents the position compensation in the tool coordinate system, K and B are the stiffness and damping matrices in the tool coordinate system, respectively, and s is the Laplace operator. i Here is the integral gain matrix. f tartet The target force and torque are in the tool coordinate system; The position compensation amount in the tool coordinate system is converted into the joint compensation amount in the joint space; Controlling the robot's joints based on the joint compensation amount includes: filtering the joint compensation amount using a tracking differentiator, outputting the filtered joint compensation amount, superimposing the filtered joint compensation amount onto a planned motion command to obtain a superimposed motion command, and using the superimposed motion command to control the robot's joints.

2. The robot force control method according to claim 1, characterized in that, Based on the rotation matrix from the robot's base coordinate system to the force sensor and the gravitational acceleration vector in the robot's base coordinate system, the static parameters of the tools carried by the robot and the static bias of the force / torque sensors are obtained, including: Based on the rotation matrix from the robot's base coordinate system to the force sensor, the gravitational acceleration vector in the robot's base coordinate system, and the parameter calculation formula, the static parameters of the tools carried by the robot and the static offset of the force / torque sensor are obtained. The parameter calculation formula is as follows: [m m r T f 0 T ] T =(A T A) -1 A T F Where m is the tool mass, mr is the tool's first-order mass torque, f0 is the static bias of the force / torque sensor, and A=[A1] T A2 T … A N T ] T A i For the i-th sampling point, [R] F 0 g 003 3I303 3; 03 1–S(R F 0 g 0) 03 3I3], R F 0 Let g be the rotation matrix from the robot's base coordinate system to the force sensor, g0 be the gravitational acceleration vector in the robot's base coordinate system, and S be the cross product operator. 3 is a third-order zero matrix, I3 is a third-order identity matrix, F=[ f 1 T ... f 2 T ... f N T ] T , f i These are the force and torque measurement data corresponding to the i-th sampling point.

3. The robot force control method according to claim 2, characterized in that, The step of obtaining the contact force and torque between the robot-carried tool and the environment in the tool coordinate system based on the static parameters of the tool carried by the robot and the static offset of the force / torque sensor includes: The static parameters of the tools carried by the robot and the static bias of the force / torque sensor are used as tool compensation values ​​to obtain force / torque sensor data. Subtract the tool compensation value from the force / torque sensor data to obtain the contact force and torque between the robot-carried tool and the environment in the sensor coordinate system; The contact forces and torques between the robot's tools and the environment in the sensor coordinate system are transformed to obtain the contact forces and torques between the robot's tools and the environment in the tool coordinate system.

4. The robot force control method according to claim 3, characterized in that, The contact forces and torques between the robot's tool and the environment in the sensor coordinate system are transformed to obtain the contact forces and torques between the robot's tool and the environment in the tool coordinate system, including: Using coordinate transformation formulas, the contact forces and torques between the robot's tool and the environment in the sensor coordinate system are transformed to obtain the contact forces and torques between the robot's tool and the environment in the tool coordinate system. The coordinate transformation formula is as follows: f F c = f -[R F 0 g 003 3 I303 3; 03 1–S(R F 0 g 0) 03 3I3]∙[m m r T f 0 T ] T f T c =H T F f F c H T F =[R T F 03 3;S( p TF ) R T F R T F ] in, f F c The contact force and torque in the sensor coordinate system f T c H represents the contact force and torque in the tool coordinate system. T F R is the transfer matrix from the sensor coordinate system to the tool coordinate system. T F Let be the rotation matrix from the sensor coordinate system to the tool coordinate system. p TF It is the vector from the origin of the sensor coordinate system to the origin of the tool coordinate system in the tool coordinate system.

5. The robot force control method according to claim 1, characterized in that, Converting the position compensation amount in the tool coordinate system to the joint compensation amount in the joint space includes: The position compensation amount in the tool coordinate system is converted into the joint compensation amount in the joint space using the compensation amount conversion formula, wherein the compensation amount conversion formula is ∆. q =sat(J -1 ∆ x , q u , q l ), ∆ q Let J be the joint compensation amount, and J be the Jacobian matrix in the tool coordinate system. q u This represents the upper limit of joint compensation. q l ∆ represents the lower limit of joint compensation. x This is the position compensation amount in the tool coordinate system.

6. The robot force control method according to claim 1, characterized in that, The formula describing the tracking differentiator is: f = wait (r s - - r in , ṙ s - ,A max ,T s ) r s + = r s - + T s ∙ṙ s - ṙ s + = sat (ṙ s - + T s ∙f, -V max , V max ) in, T s Sampling time, r s - This is the joint position compensation amount at the previous sampling time. r s + This is the joint position compensation amount at the current sampling time. ṙ s - This is the derivative of the joint position compensation amount at the previous sampling time. ṙ s + This is the derivative of the joint position compensation amount at the current sampling time. A max This is the upper bound of the second derivative of the joint position compensation. f The second derivative of the joint position compensation. V max Upper bound of the derivative of joint position compensation r in This is the input joint position compensation amount. sat It is a saturation function. fhan It is a non-linear function.

7. A robot force control device, characterized in that, It includes a contact force and torque acquisition module, a position compensation acquisition module, a joint compensation acquisition module, and a force control module; The contact force and torque acquisition module is used to acquire the contact force and torque between the robot-carried tool and the environment in the tool coordinate system. Acquiring the contact force and torque between the robot-carried tool and the environment in the tool coordinate system includes: acquiring the rotation matrix from the robot base coordinate system to the force sensor and the gravitational acceleration vector in the robot base coordinate system; acquiring the static parameters of the robot-carried tool and the static offset of the force / torque sensor based on the rotation matrix from the robot base coordinate system to the force sensor and the gravitational acceleration vector in the robot base coordinate system; and acquiring the contact force and torque between the robot-carried tool and the environment in the tool coordinate system based on the static parameters of the robot-carried tool and the static offset of the force / torque sensor. The static parameters of the robot-carried tool include the tool mass and the first-order mass moment. The position compensation acquisition module is used to acquire the position compensation amount in the tool coordinate system based on the contact force and torque in the tool coordinate system using admittance control and integral control algorithms. The formula for the admittance control and integral control algorithm is ∆ x =(K+B s ) -1 ∙( f tartet - f F c )+K i ∫( f tartet - f F c ), where ∆ x K represents the position compensation in the tool coordinate system, K and B are the stiffness and damping matrices in the tool coordinate system, respectively, and s is the Laplace operator. i Here is the integral gain matrix. f tartet The target force and torque are in the tool coordinate system; The joint compensation amount acquisition module is used to convert the position compensation amount in the tool coordinate system into the joint compensation amount in the joint space. The force control module is used to control the joints of the robot according to the joint compensation amount. The control of the robot's joints according to the joint compensation amount includes filtering the joint compensation amount using a tracking differentiator, outputting the filtered joint compensation amount, superimposing the filtered joint compensation amount into a planned motion command to obtain a superimposed motion command, and using the superimposed motion command to control the robot's joints.

8. A robot, characterized in that, The system includes a processor and a memory coupled to the processor, the memory storing program instructions executable by the processor; when the processor executes the program instructions stored in the memory, it implements the robot force control method according to any one of claims 1-6.

9. A storage medium, characterized in that, The storage medium stores program instructions, which, when executed by a processor, implement the robot force control method according to any one of claims 1-6.

Images