Constant force control method and electronic device

Abstract

The present invention provides a constant force control method and electronic device, which teaches a robot in advance on a target surface by teaching to obtain a teaching trajectory. During the control process, force data and torque detected by a force sensor at multiple consecutive moments are obtained, and a force difference value is obtained based on the force data and a preset target force. The final position adjustment increment at the next moment is obtained based on the force difference value, and the final posture adjustment increment at the next moment is obtained based on the torque. The final position adjustment increment is corrected based on the torque, and the position information and posture information of the robot at the next moment are obtained based on the final posture adjustment increment, the corrected final position adjustment increment, and the teaching trajectory prediction. In this solution, the position adjustment increment is corrected based on the torque, and combined with the posture adjustment increment to achieve constant force control for the surface, thereby ensuring a constant force effect.

Description

Technical Field

[0001] The present invention relates to the field of control technology, and in particular to a constant force control method and electronic equipment. Background Art

[0002] As the main representative of artificial intelligence, robots have been widely used in all walks of life. Among them, collaborative robots can replace industrial robots in some occasions due to their advantages such as high safety factor and low power. At present, collaborative robots need to install tools on the end of their flanges to work. Application scenarios include welding workpiece grinding, human massage, and shaft hole assembly, all of which require the end tool to be in direct contact with the object. During the operation, it is usually necessary to maintain a constant force between the end tool and the object. Therefore, it is necessary to use force sensors to achieve constant force control, such as Figure 1 as shown in .

[0003] The existing control methods mainly include control based on teaching and control based on visual recognition. Among them, the control method based on visual recognition involves tasks with relatively large computational load, such as image processing, feature extraction, and target detection, which requires powerful computing resources and thus increases the complexity of the system.

[0004] In contrast, the control method based on teaching can reduce the amount of calculation and is simple to operate. In the existing control method based on teaching, the target force direction is usually set in the opposite direction of the end flange coordinate system, and the adjustment position of the constant force control is also along the set target force direction. In this way, when the target surface is a curved surface, for example, when the collaborative robot is used for human massage, the human body surface is uneven. In the constant force control operation, the robot's end tool cannot completely fit with the target surface, which affects the user experience to a certain extent and greatly limits the application scenarios of the constant force control operation. Summary of the invention

[0005] The purpose of the embodiments of the present invention is to provide a constant force control method and electronic equipment to achieve constant force control on a curved surface and ensure a constant force effect.

[0006] In a first aspect, the present invention provides a constant force control method, which is applied to a robot, wherein a force sensor is installed at the end of the robot, and the method comprises:

[0007] Teaching the robot in advance on a target surface by a teaching method to obtain a teaching trajectory;

[0008] During the control process, the force data and torque detected by the force sensor at a plurality of consecutive moments are obtained;

[0009] Obtaining a force difference value based on the force data and a preset target force, obtaining a final position adjustment increment at the next moment according to the force difference value, and obtaining a final posture adjustment increment at the next moment based on the torque;

[0010] modifying the final position adjustment increment based on the torque;

[0011] The position information and posture information of the robot at the next moment are obtained based on the final posture adjustment increment, the corrected final position adjustment increment and the teaching trajectory prediction.

[0012] In an optional embodiment, the step of pre-teaching the robot on the target surface by teaching to obtain a teaching trajectory includes:

[0013] Recording the posture information of the robot at the highest point and the lowest point of the target surface by teaching;

[0014] The robot is controlled by adopting a straight line control command to obtain a teaching trajectory between the highest point and the lowest point.

[0015] In an optional implementation, the multiple moments include a current moment, a previous moment, and two previous moments;

[0016] The step of obtaining a force difference value based on the force data and a preset target force, and obtaining a final position adjustment increment at the next moment according to the force difference value comprises:

[0017] According to the force data at the current moment, the previous moment, and the previous two moments and the corresponding preset target forces, respectively obtain the force difference values ​​at the current moment, the previous moment, and the previous two moments;

[0018] Based on the force difference values ​​at the current moment, the previous moment, and the previous two moments, and the PID parameters of the force tracking controller, a calculated position adjustment increment is obtained;

[0019] Based on the existing position adjustment increment at the current moment and the calculated position adjustment increment, a final position adjustment increment at the next moment is obtained.

[0020] In an optional embodiment, the method further comprises:

[0021] determining whether a first parameter adjustment condition is satisfied based on the force difference value;

[0022] If the first parameter adjustment condition is met, the PID parameters of the force tracking controller are adjusted according to the set adjustment gain and the designed adjustment formula, so that the force tracking controller calculates the position adjustment increment according to the adjusted PID parameters.

[0023] In an optional implementation, the multiple moments include a current moment and a previous moment;

[0024] The step of obtaining the final posture adjustment increment at the next moment based on the torque includes:

[0025] Based on the torque at the previous moment and the torque at the current moment, obtaining torque change information;

[0026] Based on the torque change information and using an admittance control algorithm, obtaining a posture adjustment angle corresponding to the torque change information;

[0027] The final posture adjustment increment at the next moment is obtained according to the posture adjustment increment existing at the current moment and the posture adjustment angle.

[0028] In an optional embodiment, the method further comprises:

[0029] determining whether a second parameter adjustment condition is satisfied based on the force difference value;

[0030] If the second parameter adjustment condition is met, the admittance control parameter in the admittance control algorithm is adjusted according to the torque change information.

[0031] In an optional embodiment, the step of correcting the final position adjustment increment based on the torque includes:

[0032] Obtaining a posture adjustment angle based on the torque;

[0033] According to the posture adjustment angle and the obtained final position adjustment increment, a corrected final position adjustment increment is obtained.

[0034] In an optional embodiment, after the step of correcting the final position adjustment increment based on the torque, the method further includes:

[0035] Detecting whether the force data is less than a set compliance control force;

[0036] If it is smaller than the compliance control force, the corrected final position adjustment increment is corrected again based on the set compliance control coefficient.

[0037] In an optional embodiment, the step of predicting the position information and posture information of the robot at the next moment based on the final posture adjustment increment, the corrected final position adjustment increment and the teaching trajectory includes:

[0038] Obtaining a homogeneous transformation matrix of the robot at the current moment in the teaching trajectory, and updating the homogeneous transformation matrix based on the final posture adjustment increment and the corrected final position adjustment increment to obtain a homogeneous transformation matrix at the next moment;

[0039] The inverse solution of the homogeneous transformation matrix at the next moment is solved to obtain the position information and attitude information at the next moment.

[0040] In a second aspect, the present invention provides an electronic device, comprising a memory and a processor, wherein the memory stores a computer program that can be run on the processor, and when the processor executes the computer program, the steps of the method described in any one of the aforementioned implementation modes are implemented.

[0041] The present invention provides a constant force control method and electronic device, which teaches a robot in advance on a target surface by teaching to obtain a teaching trajectory. During the control process, force data and torque detected by a force sensor at multiple consecutive moments are obtained, and a force difference value is obtained based on the force data and a preset target force. The final position adjustment increment at the next moment is obtained based on the force difference value, and the final posture adjustment increment at the next moment is obtained based on the torque. The final position adjustment increment is corrected based on the torque, and the position information and posture information of the robot at the next moment are obtained based on the final posture adjustment increment, the corrected final position adjustment increment, and the teaching trajectory prediction. In this solution, the position adjustment increment is corrected based on the torque, and combined with the posture adjustment increment to achieve constant force control for the surface, thereby ensuring a constant force effect. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for use in the embodiments of the present invention are briefly introduced below. It should be understood that the following drawings only show certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other related drawings can be obtained based on these drawings without creative work.

[0043] Figure 1 This is a schematic diagram of the collaborative robot operation;

[0044] Figure 2 A flow chart of a constant force control method provided by an embodiment of the present invention;

[0045] Figure 3 It is a schematic diagram of the robot end coordinate system and the force sensor coordinate system;

[0046] Figure 4 It is a schematic diagram of the robot working surface;

[0047] Figure 5A schematic diagram of the principle of constant force control provided by an embodiment of the present invention;

[0048] Figure 6 Another schematic diagram of the principle of constant force control provided by an embodiment of the present invention;

[0049] Figure 7 for Figure 2 A flowchart of the sub-steps included in S13;

[0050] Figure 8 A schematic diagram of the forces acting on the end tool of the robot during operation in an embodiment of the present invention;

[0051] Fig. 9 for Figure 2 Another flow chart of the sub-steps included in S13;

[0052] Fig.10 Schematic diagram of the relationship between the position adjustment increment and the attitude adjustment angle in an embodiment of the present invention;

[0053] Fig.11 for Figure 2 A flowchart of the sub-steps included in S14;

[0054] Fig.12 for Figure 2 A flowchart of the sub-steps included in S15;

[0055] Fig.13 A flow chart of a PID parameter adjustment method involved in a constant force control method provided in an embodiment of the present invention;

[0056] Fig.14 A flow chart of a method for adjusting admittance control parameters involved in a constant force control method provided in an embodiment of the present invention;

[0057] Fig.15 A flow chart of a compliant control method involved in a constant force control method provided in an embodiment of the present invention;

[0058] Fig.16 A functional module block diagram of a constant force control device provided by an embodiment of the present invention;

[0059] Fig.17 This is a structural block diagram of an electronic device provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0060] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Generally, the components of the embodiments of the present invention described and shown in the drawings here can be arranged and designed in various different configurations.

[0061] Therefore, the following detailed description of the embodiments of the present invention provided in the accompanying drawings is not intended to limit the scope of the invention claimed for protection, but merely represents selected embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0062] It should be noted that similar reference numerals and letters denote similar items in the following drawings, and therefore, once an item is defined in one drawing, further definition and explanation thereof is not required in subsequent drawings.

[0063] In the description of the present invention, it should be noted that the terms “first”, “second”, etc., if used, are only used to distinguish the descriptions and cannot be understood as indicating or implying relative importance.

[0064] It should be noted that, in the absence of conflict, the features in the embodiments of the present invention may be combined with each other.

[0065] See also Figure 2 , which is a flow chart of a constant force control method provided by an embodiment of the present invention, the constant force control method can be executed by a constant force control device, the constant force control device can be implemented by software and / or hardware, and can be configured in an electronic device, the electronic device can be a device for realizing robot control, for example, it can be a computer, a controller included in the robot, and other devices. Among them, a force sensor is installed at the end of the robot, and a tool is also installed at the end of the robot, such as a tool for massage, a tool for welding, a tool for grinding, etc.

[0066] See also Figure 2 The detailed steps of the constant force control method provided by the embodiment of the present invention are introduced as follows.

[0067] S11, teaching the robot in advance on the target surface by teaching, to obtain a teaching trajectory.

[0068] S12, during the control process, obtaining force data and torque detected by the force sensor at a plurality of consecutive moments.

[0069] S13, obtaining a force difference value based on the force data and a preset target force, obtaining a final position adjustment increment at the next moment according to the force difference value, and obtaining a final posture adjustment increment at the next moment based on the torque.

[0070] S14, correcting the final position adjustment increment based on the torque.

[0071] S15, predicting the position information and posture information of the robot at the next moment based on the final posture adjustment increment, the corrected final position adjustment increment and the teaching trajectory.

[0072] In this embodiment, the robot may be a six-degree-of-freedom collaborative robot or other multi-degree-of-freedom robots, and is not specifically limited. The force sensor may be a six-dimensional force sensor. The force sensor is mounted on the end flange of the robot. When the direction of the force sensor is inconsistent with the direction of the end flange of the robot, for example Figure 3 As shown in , at this time, it is necessary to establish a homogeneous transformation matrix from the force sensor coordinate system to the end coordinate system.

[0073] Specifically, the transformation relationship from the force sensor coordinate system to the end coordinate system can be set ,in, x , y , z Represents the translation relationship between the force sensor coordinate system and the end coordinate system, a , b , c They are the distance between the force sensor coordinate system and the end coordinate system. x , y , z Based on this, the homogeneous transformation matrix from the force sensor coordinate system to the end coordinate system can be established as shown below: :

[0074]

[0075] in, and are the rotation matrix and translation matrix from the force sensor coordinate system to the end coordinate system, respectively.

[0076] Based on this, it is assumed that the data detected by the force sensor is expressed in the force sensor coordinate system as , then after conversion to the terminal coordinate system, the data is converted to :

[0077]

[0078] in, , is a zero matrix.

[0079] On this basis, tools are also installed on the end flange of the robot. After the tools are installed, the zero point calibration operation of the force sensor is completed.

[0080] In constant force control operations, it is necessary to obtain the torque information generated by the end tool and the contact surface. Therefore, it is necessary to convert the data detected by the force sensor from the end coordinate system to the tool coordinate system.

[0081] Assume that the offset information of the robot tool relative to the end flange is: , then the transformation relationship from the force sensor coordinate system to the tool coordinate system is as follows (the torque introduced by the offset between the tool and the end flange needs to be considered):

[0082]

[0083] in, , are the rotation matrix and translation matrix from the end coordinate system to the tool coordinate system, Represents the cross product operator.

[0084] In this embodiment, the target surface can be Figure 4 In the actual application scenario, the target surface can be understood as an uneven surface such as a workpiece surface or a human body surface. The robot can be taught in advance with respect to the target surface to obtain a teaching trajectory.

[0085] Specifically, teaching can be carried out in the following ways:

[0086] The position information of the robot at the highest point and the lowest point of the target surface is recorded by teaching; the robot is controlled by a linear control command to obtain the teaching trajectory between the highest point and the lowest point.

[0087] Through the above teaching method, you only need to record the posture information of the robot at the highest and lowest points. Combined with the linear control command, the teaching trajectory can be obtained. It can be understood that if the robot runs completely according to the teaching trajectory, when the robot encounters uneven positions on the surface, the force between the tool on the robot and the target surface is variable, and the effect of constant force cannot be achieved.

[0088] Therefore, it is necessary to make adjustments based on the taught trajectory so that the force between the tool on the robot and the target surface remains constant.

[0089] In this embodiment, when the constant force control is turned on, during the control process, the force sensor can sample the force data and torque according to the sampling interval. The sampling interval can be set according to the needs, for example, it can be 1 second, 2 seconds, etc.

[0090] The control of the robot is real-time control. Therefore, taking the current moment as an example, it is necessary to predict the control information of the next moment based on the sampling information of the current moment and the previous moment, and then adjust the teaching trajectory based on the predicted control information of the next moment, so that the robot runs according to the adjusted information to achieve the purpose of ensuring constant force control.

[0091] In this embodiment, based on the teaching trajectory, when only the constant force control is turned on, Figure 5 As shown in , only the position information in the teaching trajectory is corrected. When constant force control and posture adjustment are turned on, as shown in Figure 6 As shown in , it is necessary to adjust the position information and posture information in the teaching trajectory to ensure that the tool on the robot is always along the normal direction of the target surface to ensure fit with the target surface. Figure 6 middle F d represents the preset target force, F s Represents the force data detected by the force sensor. surface F d and F s The force difference between x 1 Represents the position adjustment increment obtained, M s represents the torque detected by the force sensor, x 2 Indicates the obtained attitude adjustment increment, x d Represents the data in the teaching trajectory, and S1 and S2 represent the constant force control selection marks respectively.

[0092] Based on this, in this embodiment, the force data and torque of the force sensor at multiple consecutive moments obtained may be the force data and torque at the current moment, the previous moment, the previous two moments, and so on.

[0093] In addition, a preset target force is provided in this embodiment, and the preset target force can be understood as a constant force, and its control goal is to make the difference between the actual force between the tool of the control robot and the target surface and the preset target force as small as possible, or even consistent.

[0094] Based on this, in this embodiment, the force difference value is obtained based on the force data at each moment and the preset target force, and the final position adjustment increment at the next moment is obtained according to the force difference value. And the final posture adjustment increment at the next moment is obtained based on the torque.

[0095] In this embodiment, the position adjustment increment is the superposition result of multiple moments, that is, for each moment, the moment has an existing position adjustment increment, and the calculated position adjustment increment is obtained based on the calculation. The calculated position adjustment increment is superimposed on the existing position adjustment increment to obtain the final position adjustment increment, and the final position adjustment increment is used as the position adjustment increment for the next moment control.

[0096] Similarly, the attitude adjustment increment is also divided into the attitude adjustment increment existing at each moment, the calculated attitude adjustment increment, and the final attitude adjustment increment.

[0097] When posture adjustment is turned on, the posture adjustment will affect the position information of the robot. Therefore, in this embodiment, the final position adjustment increment is corrected based on the torque, so as to ensure the constant force control effect while performing posture adjustment at the same time.

[0098] On this basis, the position information and posture information of the robot at the next moment can be obtained based on the final posture adjustment increment, the corrected final position adjustment increment and the teaching trajectory. Therefore, the robot can be controlled at the next moment based on the obtained position information and posture information at the next moment. On the basis of ensuring a constant force between the tool on the robot and the target surface, the posture of the robot end is adjusted to increase the contact area between the tool on the robot end and the target surface, thereby improving the work efficiency and enhancing the user experience.

[0099] See also Figure 7 In this embodiment, the above-mentioned step of obtaining the force difference value based on the force data and the preset target force, and obtaining the final position adjustment increment at the next moment according to the force difference value can be implemented in the following manner:

[0100] S131, according to the force data at the current moment, the previous moment and the previous two moments and their corresponding preset target forces, respectively obtain the force difference values ​​at the current moment, the previous moment and the previous two moments.

[0101] S132, obtaining a calculated position adjustment increment based on the force difference values ​​at the current moment, the previous moment, and the previous two moments, and the PID parameters of the force tracking controller.

[0102] S133, obtaining a final position adjustment increment at the next moment based on the existing position adjustment increment at the current moment and the calculated position adjustment increment.

[0103] In this embodiment, based on obtaining the force difference values ​​corresponding to the current moment, the previous moment, and the previous two moments, respectively, the calculated position adjustment increment generated by the PID control can be obtained by calculation according to the following formula:

[0104]

[0105] in, , and The PID parameters include proportional coefficient, integral coefficient and differential coefficient. , and is the force difference corresponding to the current moment, the previous moment, and the previous two moments respectively.

[0106] As can be seen from the above, the calculated position adjustment increment is only an incremental value based on the current position adjustment increment obtained by PID control. Therefore, it is also necessary to add the calculated position adjustment increment to the existing position adjustment increment at the current moment to obtain the final position adjustment increment to be generated at the next moment.

[0107] The above is only the position adjustment increment obtained from the perspective of maintaining constant force. In order to keep the tool completely fitted with the target surface, posture adjustment is required. Assuming that the Z direction of the robot end is selected as the constant force control position adjustment direction, the control direction of the posture adjustment is the rotation direction around the X-axis and Y-axis.

[0108] like Figure 6 The S1 and S2 shown in the figure represent constant force control selection flags respectively. Based on the above assumption selection information, S1 and S2 are respectively represented as follows:

[0109]

[0110] against Figure 8 As shown in the force analysis diagram of the end tool operation, assuming that the X direction of the robot end is consistent with the movement direction, the friction force generated by the end tool and the target surface will affect the force sensor. Have an impact, such as Figure 8 As shown on the left, ,like Figure 8 As shown on the right side of Similarly, the force analysis of the robot end in the Y direction is consistent with that in the X direction.

[0111] Based on this, see Fig. 9 , in the process of performing attitude adjustment, the step of obtaining the final attitude adjustment increment based on the torque can be achieved by the following methods:

[0112] S134, obtaining torque change information based on the torque at the previous moment and the torque at the current moment.

[0113] S135: Based on the torque change information and using an admittance control algorithm, obtain a posture adjustment angle corresponding to the torque change information.

[0114] S136, obtaining a final posture adjustment increment at the next moment according to the posture adjustment increment existing at the current moment and the posture adjustment angle.

[0115] In this embodiment, the admittance control algorithm can be used to establish the transformation relationship between the torque and the attitude angle as shown below:

[0116]

[0117] in, is the inertia matrix, is the damping matrix, is the stiffness matrix, is the force data detected by the force sensor at the current moment, and is the position at the current moment and the previous moment, and is the speed at the current moment and the previous moment, is the acceleration generated by the external force at the current moment, is the time interval.

[0118] When contacting an uneven position on the target surface, the following will occur when contacting and moving away from the position: Fig.10 Therefore, based on the above-mentioned admittance control algorithm, the attitude adjustment angle corresponding to the torque change under the unknown surface can be obtained, including and Among them, the torque change information is obtained based on the torque at the previous moment and the torque at the current moment, and can be reflected by the posture at the current moment and the posture at the previous moment.

[0119] Assume that the current attitude adjustment increment is , then on this basis, according to the obtained attitude adjustment angle, the final attitude adjustment increment at the next moment can be obtained as shown below:

[0120]

[0121] On the basis of enabling attitude adjustment, the position adjustment increment generated by constant force control needs to be combined with the attitude adjustment angle, such as Fig.10 As shown in, is the position adjustment increment. When the posture adjustment is turned on, if Not adjusted to , it will cause the end of the robot to be closer to the contact object, thereby generating a greater squeezing force, causing a collision or danger.

[0122] Therefore, please refer to Fig.11, when correcting the final position adjustment increment based on the torque, it can be achieved in the following ways:

[0123] S141, obtaining a posture adjustment angle based on the torque.

[0124] S142, obtaining a corrected final position adjustment increment according to the posture adjustment angle and the obtained final position adjustment increment.

[0125] In this embodiment, the posture adjustment angle can be obtained based on the above method, and the posture adjustment angle and The rotation matrix can be constructed :

[0126]

[0127] Among them, roty( ) indicates that the coordinate system revolves around y Axis rotation angle The resulting rotation matrix is ​​returned as The matrix, rotx( ) indicates that the coordinate system revolves around x Axis rotation angle The resulting rotation matrix is ​​returned as The matrix of .

[0128] Based on the final position adjustment increment and attitude adjustment angle, the corrected final position adjustment increment can be obtained according to the following formula:

[0129]

[0130] in, The final position adjustment increment (Z direction) for the next moment after correction, is the calculated position adjustment increment (Z direction) obtained by PID control at the current moment, is the position adjustment increment (Z direction) at the current moment. It can be understood that the position adjustment increment at the current moment and the calculated position adjustment increment can be used to obtain the position adjustment increment before correction. On this basis, combined with The position adjustment increment before correction can be corrected to obtain the position adjustment increment after correction. .

[0131] Based on the above method, the final attitude adjustment increment and the corrected final position adjustment increment can be obtained. In this way, the position information and attitude information of the next moment can be obtained based on the teaching trajectory, and then the control of the next moment can be realized. Fig.12 In this embodiment, this step can be implemented in the following way:

[0132] S151, obtaining the homogeneous transformation matrix of the robot at the current moment in the teaching trajectory, and updating the homogeneous transformation matrix based on the final posture adjustment increment and the corrected final position adjustment increment to obtain the homogeneous transformation matrix at the next moment.

[0133] S152, performing an inverse solution on the homogeneous transformation matrix at the next moment to obtain the position information and posture information at the next moment.

[0134] In this embodiment, the final posture adjustment increment obtained above is , , And the final position adjustment increment after correction Together they form a homogeneous transformation matrix Based on the teaching trajectory, the homogeneous transformation matrix of the robot at the current moment can be obtained On this basis, the homogeneous transformation matrix at the next moment can be calculated according to the following formula: :

[0135]

[0136] By the homogeneous transformation matrix of the next moment By performing the inverse solution, the position and posture information of each joint of the robot at the next moment can be obtained, and then the robot can be controlled at the next moment based on the calculated position and posture information.

[0137] In this embodiment, during the process of controlling the robot, the force data of the force sensor fluctuates around the preset target force when the end of the robot contacts the target surface. However, when the target surface fluctuates, the force data of the force sensor will have a spike. At this time, if a fixed PID parameter is used at constant force, the robot will respond too slowly. For example, when the robot is used in the massage field, it will produce a bad user experience. Therefore, please refer to Fig.13 The constant force control method provided in this embodiment further includes the following steps:

[0138] S21: Determine whether a first parameter adjustment condition is met based on the force difference value.

[0139] S22: If the first parameter adjustment condition is met, the PID parameters of the force tracking controller are adjusted according to the set adjustment gain and the designed adjustment formula, so that the force tracking controller calculates the position adjustment increment according to the adjusted PID parameters.

[0140] In this embodiment, in order to avoid too frequent adjustment actions, the force difference value may be first subjected to a limiting filter process. Specifically, when the force difference value is less than or equal to a first preset threshold value, half of the first preset threshold value is taken as the equivalent error. When the force difference value is greater than the first preset threshold value and less than or equal to the second preset threshold value, the first preset threshold value is taken as the equivalent error.

[0141] When the equivalent error is less than or equal to the adjustment threshold, it is determined that the first parameter adjustment condition is not met, that is, there is no need to adjust the PID parameters. When the equivalent error is greater than the adjustment threshold, it is determined that the first parameter adjustment condition is met.

[0142] When the first parameter adjustment condition is met, the adjustment gain and equivalent error based on the setting And adjust the PID parameters according to the formula shown below:

[0143]

[0144] in, and To set the fixed PID parameters, and The adjustment gain of the parameter P is set, and The adjustment gain of parameter I is set.

[0145] In this embodiment, only PI control is performed, so only the parameters P and I in the PID parameters are adjusted. In the process of constant force control, when the system error is large in the initial adjustment, it is necessary to increase , reduce , thereby speeding up the response and reducing the overshoot. When the system response is close to a stable state, reduce ,Increase , thereby achieving the effect of actively eliminating steady-state errors and improving the steady-state accuracy of the system. After the adaptive PID parameters are adjusted, the system steady-state error is reduced, the system anti-interference ability is enhanced, and the constant force control effect is improved.

[0146] In addition, considering that the PID control algorithm still has overshoot, when the overshoot force is too large, the reverse adjustment force will be too large, causing the robot end to collide with the contact object. Therefore, it is also necessary to adjust the admittance control parameters in the admittance control algorithm. Based on this, please refer to Fig.14 The constant force control method provided in this embodiment may further include the following steps:

[0147] S31: Determine whether a second parameter adjustment condition is met based on the force difference value.

[0148] S32: If the second parameter adjustment condition is met, the admittance control parameter in the admittance control algorithm is adjusted according to the torque change information.

[0149] In this embodiment, the method of judging whether the second parameter adjustment condition is satisfied based on the force difference value is the same as the above-mentioned method of judging whether the first parameter adjustment condition is satisfied based on the force difference value, and will not be elaborated here.

[0150] When the second parameter adjustment condition is met, the admittance control parameter is adjusted according to the torque change information and the following formula:

[0151]

[0152] in, is the initial value of the damping parameter in the admittance control parameter, To adjust the damping parameters later, is the proportional gain, is the differential gain, and is the torque detected by the force sensor, and It is the torque change information between two adjacent moments.

[0153] When the robot is running, if the curvature of the target surface changes, or However, when the damping in the corresponding direction is increased, the adjustment speed of the admittance control algorithm will be reduced, ensuring that the attitude adjustment angle and It will not change drastically, ensuring the stability of the system in posture adjustment, thereby achieving surface tracking.

[0154] Considering that when robots are used in the massage field, the human body has a certain flexibility, which makes the constant force control system easily changed by external forces. Based on this, please refer to Fig.15 In this embodiment, after the final position adjustment increment is corrected based on the torque, the following steps may also be included:

[0155] S41, detecting whether the force data is less than a set compliance control force.

[0156] S42: If the value is smaller than the compliance control force, the corrected final position adjustment increment is corrected again based on the set compliance control coefficient.

[0157] In this embodiment, when the force data detected by the force sensor is less than the set compliance control force, the position adjustment increment calculated based on the above method can be corrected again, and the correction can be made according to the following formula:

[0158]

[0159] in, is the compliance control coefficient, The position adjustment increment (Z direction) after the correction again. The final position adjustment increment (Z direction) at the next moment after the correction obtained above.

[0160] Based on the above-mentioned re-correction processing, the position adjustment increment generated by the PID algorithm can be reduced, so that the robot end tool can better contact with the target surface and achieve stable contact.

[0161] In summary, the constant force control method provided in this embodiment is based on the torque information detected by the force sensor, and through the admittance control algorithm, the torque change information is converted into the attitude adjustment angle, and the position adjustment increment generated by the PID control is combined with the attitude adjustment angle to obtain the final position adjustment increment, and together realize the constant force control for the unknown surface. Compared with the existing traditional constant force control scheme, the contact surface condition can be judged according to the torque change in this scheme, and then the position adjustment increment can be corrected based on the attitude adjustment angle to realize the surface fitting during the constant force control process.

[0162] In addition, in this scheme, the PID parameters and the admittance control parameters are adjusted. According to the rate of change of the force error, the PID parameters are adjusted. When the force error is large, the response speed can be increased, and when the force error is small, the steady-state error can be reduced. According to the torque and the rate of change of the torque, the admittance control parameters are adjusted. When the torque changes drastically, the damping is increased and the adjustment speed is reduced. Through the adaptive adjustment algorithm, constant force control can be achieved on unknown surfaces. Moreover, constant force control has the advantages of fast response speed, strong anti-interference ability and small steady-state error.

[0163] Furthermore, in this scheme, a compliant control method is designed. When the force sensor data does not exceed the set compliance force, the position adjustment amount of the constant force control output is reduced, so that the constant force control system is not easily changed, thereby enhancing the anti-interference ability of the constant force control algorithm, ensuring stable contact between the robot's end tool and the contact surface, and improving the stability of the entire system.

[0164] Based on the same inventive concept, please refer to Fig.16, an embodiment of the present invention further provides a functional module schematic diagram of a constant force control device. This embodiment can divide the functional modules of the constant force control device according to the above method embodiment. For example, each functional module can be divided according to each function, or two or more functions can be integrated into one processing module. The above integrated module can be implemented in the form of hardware or in the form of software functional modules. It should be noted that the division of modules in the embodiment of the present invention is schematic, which is only a logical function division, and there may be other division methods in actual implementation.

[0165] For example, when each functional module is divided into corresponding functional modules, Fig.16 The constant force control device shown is only a schematic diagram of the device. The constant force control device may include a teaching module, an acquisition module, a calculation module, a correction module and a control module. The functions of each functional module of the constant force control device are described in detail below.

[0166] A teaching module, used to teach the robot in advance on a target surface by a teaching method to obtain a teaching trajectory;

[0167] An acquisition module, used for acquiring force data and torque at a plurality of consecutive moments detected by the force sensor during a control process;

[0168] a calculation module, configured to obtain a force difference value based on the force data and a preset target force, obtain a final position adjustment increment at the next moment according to the force difference value, and obtain a final posture adjustment increment at the next moment based on the torque;

[0169] A correction module, configured to correct the final position adjustment increment based on the torque;

[0170] The control module is used to predict the position information and posture information of the robot at the next moment based on the final posture adjustment increment, the corrected final position adjustment increment and the teaching trajectory.

[0171] It can be understood that the above-mentioned teaching module, acquisition module, calculation module, correction module and control module can be used to execute the above-mentioned S11 to S15. The detailed implementation method of the teaching module, acquisition module, calculation module, correction module and control module can refer to the relevant contents of the above-mentioned S11 to S15.

[0172] In a possible implementation, the above teaching module can be used for:

[0173] Recording the posture information of the robot at the highest point and the lowest point of the target surface by teaching;

[0174] The robot is controlled by adopting a straight line control command to obtain a teaching trajectory between the highest point and the lowest point.

[0175] In a possible implementation, the multiple moments include a current moment, a previous moment, and two previous moments; and the calculation module may be used to:

[0176] According to the force data at the current moment, the previous moment, and the previous two moments and the corresponding preset target forces, respectively obtain the force difference values ​​at the current moment, the previous moment, and the previous two moments;

[0177] Based on the force difference values ​​at the current moment, the previous moment, and the previous two moments, and the PID parameters of the force tracking controller, a calculated position adjustment increment is obtained;

[0178] Based on the existing position adjustment increment at the current moment and the calculated position adjustment increment, a final position adjustment increment at the next moment is obtained.

[0179] In a possible implementation, the constant force control device further includes an adjustment module, which can be used to:

[0180] determining whether a first parameter adjustment condition is satisfied based on the force difference value;

[0181] If the first parameter adjustment condition is met, the PID parameters of the force tracking controller are adjusted according to the set adjustment gain and the designed adjustment formula, so that the force tracking controller calculates the position adjustment increment according to the adjusted PID parameters.

[0182] In a possible implementation, the multiple moments include a current moment and a previous moment; and the calculation module may be used to:

[0183] Based on the torque at the previous moment and the torque at the current moment, obtaining torque change information;

[0184] Based on the torque change information and using an admittance control algorithm, obtaining a posture adjustment angle corresponding to the torque change information;

[0185] The final posture adjustment increment at the next moment is obtained according to the posture adjustment increment existing at the current moment and the posture adjustment angle.

[0186] In a possible implementation, the adjustment module may also be used to:

[0187] determining whether a second parameter adjustment condition is satisfied based on the force difference value;

[0188] If the second parameter adjustment condition is met, the admittance control parameter in the admittance control algorithm is adjusted according to the torque change information.

[0189] In a possible implementation, the correction module may be used to:

[0190] Obtaining a posture adjustment angle based on the torque;

[0191] According to the posture adjustment angle and the obtained final position adjustment increment, a corrected final position adjustment increment is obtained.

[0192] In a possible implementation, the correction module may also be used to:

[0193] Detecting whether the force data is less than a set compliance control force;

[0194] If it is smaller than the compliance control force, the corrected final position adjustment increment is corrected again based on the set compliance control coefficient.

[0195] In a possible implementation, the control module may be used to:

[0196] Obtaining a homogeneous transformation matrix of the robot at the current moment in the teaching trajectory, and updating the homogeneous transformation matrix based on the final posture adjustment increment and the corrected final position adjustment increment to obtain a homogeneous transformation matrix at the next moment;

[0197] The inverse solution of the homogeneous transformation matrix at the next moment is solved to obtain the position information and attitude information at the next moment.

[0198] See also Fig.17 , is a block diagram of an electronic device provided in an embodiment of the present invention, the electronic device may be a computer device, etc., and the electronic device includes a memory, a processor, and a communication module. The memory, the processor, and the communication module are electrically connected to each other directly or indirectly to achieve data transmission or interaction. For example, these components can be electrically connected to each other via one or more communication buses or signal lines.

[0199] The memory is used to store computer programs or data. The memory can be, but is not limited to, random access memory (RAM), read only memory (ROM), programmable read-only memory (PROM), erasable programmable read-only memory (EPROM), electrically erasable programmable read-only memory (EEPROM), etc.

[0200] The processor is used to read / write data or programs stored in the memory and execute the constant force control method provided by any embodiment of the present invention.

[0201] The communication module is used to establish a communication connection between the electronic device and other communication terminals through the network, and is used to send and receive data through the network.

[0202] It should be understood that Fig.17 The structure shown is only a schematic diagram of the structure of the electronic device. The electronic device may also include Fig.17 More or fewer components as shown, or with Fig.17 Different configurations are shown.

[0203] Furthermore, an embodiment of the present invention also provides a computer-readable storage medium, which stores machine-executable instructions. When the machine-executable instructions are executed, the constant force control method provided in the above embodiment is implemented.

[0204] Specifically, the computer-readable storage medium can be a general storage medium, such as a mobile disk, a hard disk, etc. When the computer program on the computer-readable storage medium is executed, the above-mentioned constant force control method can be executed. Regarding the process involved when the computer-readable storage medium and its executable instructions are executed, reference can be made to the relevant description in the above-mentioned method embodiment, which will not be described in detail here.

[0205] In summary, the constant force control method and electronic device provided by the embodiments of the present invention obtain a teaching trajectory by pre-teaching the robot on the target surface through a teaching method. During the control process, the force data and torque detected by the force sensor at multiple consecutive moments are obtained, and the force difference value is obtained based on the force data and the preset target force. The final position adjustment increment at the next moment is obtained according to the force difference, and the final posture adjustment increment at the next moment is obtained based on the torque. The final position adjustment increment is corrected based on the torque, and the position information and posture information of the robot at the next moment are obtained based on the final posture adjustment increment, the corrected final position adjustment increment, and the teaching trajectory prediction. In this solution, the position adjustment increment is corrected based on the torque, and combined with the posture adjustment increment to achieve constant force control for the surface and ensure the constant force effect.

[0206] The above is only a specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily thought of by a person skilled in the art within the technical scope disclosed by the present invention should be included in the protection scope of the present invention. Therefore, the protection scope of the present invention should be based on the protection scope of the claims.

Claims

1. A constant force control method, characterized in that: Applied to a robot, wherein a force sensor is installed at the end of the robot, the method comprises: Teaching the robot in advance on a target surface by a teaching method to obtain a teaching trajectory; During the control process, the force data and torque detected by the force sensor at a plurality of consecutive moments are obtained; Obtaining a force difference value based on the force data and a preset target force, obtaining a final position adjustment increment at the next moment according to the force difference value, and obtaining a final posture adjustment increment at the next moment based on the torque; modifying the final position adjustment increment based on the torque; Predicting the position information and posture information of the robot at the next moment based on the final posture adjustment increment, the corrected final position adjustment increment and the teaching trajectory; The multiple moments include the current moment, the previous moment, and the previous two moments, and the step of obtaining the final position adjustment increment at the next moment includes: According to the force data at the current moment, the previous moment and the previous two moments and the corresponding preset target forces, the force difference values ​​at the current moment, the previous moment and the previous two moments are obtained respectively; based on the force difference values ​​at the current moment, the previous moment and the previous two moments and the PID parameters of the force tracking controller, the calculated position adjustment increment is obtained; based on the existing position adjustment increment at the current moment and the calculated position adjustment increment, the final position adjustment increment at the next moment is obtained; The steps to obtain the final posture adjustment increment include: Based on the torque at the previous moment and the torque at the current moment, obtaining torque change information; based on the torque change information and using an admittance control algorithm, obtaining a posture adjustment angle corresponding to the torque change information; Obtaining a final posture adjustment increment at the next moment according to the posture adjustment increment existing at the current moment and the posture adjustment angle; The steps for correcting the final position adjustment increment include: A posture adjustment angle is obtained based on the torque; and a corrected final position adjustment increment is obtained according to the posture adjustment angle and the obtained final position adjustment increment.

2. The constant force control method according to claim 1, characterized in that: The step of pre-teaching the robot on the target surface by teaching to obtain a teaching trajectory includes: Recording the posture information of the robot at the highest point and the lowest point of the target surface by teaching; The robot is controlled by adopting a straight line control command to obtain a teaching trajectory between the highest point and the lowest point.

3. The constant force control method according to claim 1, characterized in that: The method further comprises: determining whether a first parameter adjustment condition is satisfied based on the force difference value; If the first parameter adjustment condition is met, the PID parameters of the force tracking controller are adjusted according to the set adjustment gain and the designed adjustment formula, so that the force tracking controller calculates the position adjustment increment according to the adjusted PID parameters.

4. The constant force control method according to claim 1, characterized in that: The method further comprises: determining whether a second parameter adjustment condition is satisfied based on the force difference value; If the second parameter adjustment condition is met, the admittance control parameter in the admittance control algorithm is adjusted according to the torque change information.

5. The constant force control method according to claim 1, characterized in that: After the step of correcting the final position adjustment increment based on the torque, the method further comprises: Detecting whether the force data is less than a set compliance control force; If it is smaller than the compliance control force, the corrected final position adjustment increment is corrected again based on the set compliance control coefficient.

6. The constant force control method according to any one of claims 1 to 5, characterized in that: The step of predicting the position information and posture information of the robot at the next moment based on the final posture adjustment increment, the corrected final position adjustment increment and the teaching trajectory includes: Obtaining a homogeneous transformation matrix of the robot at the current moment in the teaching trajectory, and updating the homogeneous transformation matrix based on the final posture adjustment increment and the corrected final position adjustment increment to obtain a homogeneous transformation matrix at the next moment; The inverse solution of the homogeneous transformation matrix at the next moment is solved to obtain the position information and attitude information at the next moment.

7. An electronic device comprising a memory and a processor, wherein the memory stores a computer program that can be run on the processor, characterized in that: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 6 are implemented.

Images