Self-adaptive force control method based on six-dimensional force sensor at tail end of mechanical arm

Abstract

The invention provides a self-adaptive force control method based on a six-dimensional force sensor at the tail end of a mechanical arm, and the method comprises the following steps: S1, building a load compensation model based on the six-dimensional force sensor, and calculating a gravity matrix and a zero drift force matrix; s2, on the basis of the step S1, compensating the force and the torque, and establishing a force control model; and S3, on the basis of the step S2, self-adaptive parameters are introduced, the contact force with the environment is converted into a motion control signal of the mechanical arm, and constant force control is achieved. By establishing the load model and zero drift compensation, the mechanical arm can still work stably and efficiently under the on-load condition, meanwhile, self-adaptive parameters are introduced into the impedance control algorithm, and the accuracy and stability of force control are effectively improved.

Description

Technical Field

[0001] The present invention relates to the field of robot-related technologies, and in particular to an adaptive force control method based on a six-dimensional force sensor at the end of a robotic arm. Background Art

[0002] In some simple scenarios such as handling and palletizing, these functions can be achieved by sending simple position instructions to the robot arm. However, in situations such as grinding and flexible assembly, the robot arm is also required to meet the requirements of force control. When the robot arm contacts the environment, the movement of the end of the robot arm will be constrained by the environment. Therefore, when studying the force and position control of the robot arm, the contact force between the robot arm and the environment should be considered comprehensively. Most of the external force estimation of the robot arm is based on current feedback, but the accuracy of this external force estimation is not high. The introduction of six-dimensional force sensors can improve these deficiencies and improve the accuracy of force control. Although the external force can be estimated based on the sensor, when the robot arm carries a workpiece, that is, the end is equipped with a load, the impact of the load gravity on the data of the six-dimensional force sensor will change with the movement of the robot, and the sensor noise jitter cannot be ignored. In an unknown environment, the force control is not stable enough, and it is difficult to ensure the expected force and achieve force compliance control, which especially limits the application of the robot arm in precision assembly scenarios.

[0003] Therefore, based on the above problems, the present invention provides an adaptive force control method based on a six-dimensional force sensor at the end of a robotic arm. Summary of the invention

[0004] In view of the deficiencies in the prior art, the present invention provides an adaptive force control method based on a six-dimensional force sensor at the end of a robotic arm, which solves the problems mentioned in the background technology.

[0005] To achieve the above objectives, the present invention is implemented through the following technical solutions:

[0006] An adaptive force control method based on a six-dimensional force sensor at the end of a robotic arm comprises the following steps:

[0007] S1. Based on the six-dimensional force sensor, a load compensation model is established to calculate the gravity matrix and zero drift force matrix;

[0008] S2, based on step S1, compensating the force and torque and establishing a force control model;

[0009] S3. Based on step S2, adaptive parameters are introduced to convert the contact force with the environment into a motion control signal of the robot arm to achieve constant force control.

[0010] Further, based on step S1, the load model is introduced into the formula:

[0011]

[0012] Further, based on step S2, the force and moment formula is:

[0013]

[0014]

[0015] The combined formula is as follows:

[0016]

[0017] Further, based on step S1, the feedback force collected by the six-dimensional force sensor is used to compensate gravity and zero drift for force and torque to obtain the force and torque under the base coordinates. The force collected by the sensor needs to be filtered to optimize the signal.

[0018] Furthermore, based on the force collected by the sensor, the force and torque signals are processed by IIR filter after filtering and optimization, and the transfer function is:

[0019]

[0020] The S diagonal matrix is ​​introduced, and the elements on the diagonal are 0 or 1. It is used to constrain the control mode of each degree of freedom in the coordinate system to be position control or force control, and bring the compensated force and torque into impedance control:

[0021]

[0022] Where M, B and K represent the inertia matrix, damping matrix and rigidity matrix respectively. and x are the terminal acceleration, terminal velocity and terminal position respectively, and x d are the expected acceleration, expected velocity and expected position, respectively, and F d It is the power of expectation.

[0023] Furthermore, based on step S2, the adaptive parameters σ and δ are introduced in step S3. The force and position conversion expressions of the manipulator after the adaptive parameters are introduced are:

[0024]

[0025] The present invention provides an adaptive force control method based on a six-dimensional force sensor at the end of a robotic arm. Compared with the prior art, it has the following beneficial effects:

[0026] 1. The present invention installs a six-dimensional force sensor at the end of the mechanical arm, establishes a load model and a zero-point force matrix, and eliminates the influence of load gravity and zero-point drift on the six-dimensional force sensor in real time;

[0027] 2. The present invention optimizes the force signal through filtering and processing, introduces an adaptive impedance control algorithm to achieve force control, improves the safety and responsiveness of the robotic arm, effectively achieves force control in complex environments, has a wide range of adaptability, and is reliable in operation. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0029] Figure 1 The flowchart of the adaptive force control process of the present invention is shown;

[0030] Figure 2 The force control framework diagram of the present invention is shown; DETAILED DESCRIPTION

[0031] 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 are clearly and completely described. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. 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.

[0032] Example

[0033] In order to solve the technical problems in the background technology, the following adaptive force control method based on a six-dimensional force sensor at the end of a robotic arm is provided:

[0034] Combination Figure 1-2 As shown, the present invention provides an adaptive force control method based on a six-dimensional force sensor at the end of a robotic arm, comprising the following steps:

[0035] S1. Based on the six-dimensional force sensor, a load compensation model is established to calculate the gravity matrix and zero drift force matrix;

[0036] S2, based on step S1, compensating the force and torque and establishing a force control model;

[0037] S3. Based on step S2, adaptive parameters are introduced to convert the contact force with the environment into a motion control signal of the robot arm to achieve constant force control.

[0038] Based on step S1 and step S2, as the load posture changes during the movement of the robot arm, and the direction of gravity is always vertically downward, the impact of the load gravity on the data of the six-dimensional force sensor changes with the movement of the robot and needs to be eliminated in real time. In order to obtain accurate external force, especially when the six-dimensional force sensor is installed with a tool load, the actual external force applied needs to be compensated for gravity. The six-dimensional force sensor contains three coordinates of X, Y, and Z in space. If the gravity of the load is G, its center of mass coordinates are (x, y, z). The forces generated on the X-axis, Y-axis, and Z-axis are G respectively. x , G y , G z The corresponding moments are M gx 、M gy 、M gz The load model for the robot arm is:

[0039] The load model introduction formula is:

[0040]

[0041] When there is no external force, the force and torque measured by the sensor are the force F generated by the load gravity and the sensor zero drift. x0 、F y0 、F z0 and moment M x0 、M y0 、M z0 The force and moment formulas are:

[0042]

[0043]

[0044] The combined formula is as follows:

[0045]

[0046] The six-dimensional force sensor coordinate system is not the same as the robotic arm end coordinate system, so the force and torque feedback from the sensor need to be obtained by rotation matrix transformation first, and the compensated force and torque are used to solve the posture.

[0047] Further, based on step S1, the torque obtained by the six-dimensional force sensor is not the actual external torque, which conforms to the expression F s =F e +F g +F0, where F s is the force collected by the sensor, F e is the actual external force, F gis gravity, F0 is the force generated by the zero drift of the sensor. In step S1, the feedback force collected by the six-dimensional force sensor compensates for gravity and zero drift by force and torque to obtain the force and torque under the base coordinates. The force collected by the sensor needs to be filtered and optimized. Based on the force collected by the sensor, the force and torque signals are processed by IIR filter, and the transfer function is:

[0048]

[0049] The S diagonal matrix is ​​introduced, and the elements on the diagonal are 0 or 1. It is used to constrain the control mode of each degree of freedom in the coordinate system to be position control or force control, and bring the compensated force and torque into impedance control:

[0050]

[0051] Where M, B and K represent the inertia matrix, damping matrix and rigidity matrix respectively. and x are the terminal acceleration, terminal velocity and terminal position respectively, and x d are the expected acceleration, expected velocity and expected position, respectively, and F d It is the power of expectation.

[0052] Based on step S2, in step S3, on the basis of the resistance control principle, the adaptive parameters σ and δ are introduced. The parameter σ affects the speed of error correction in force control. It is multiplied by the difference between the expected force and the actual contact force. To control the deviation and solve the influence of the uncertainty of the environmental position and stiffness on the force control accuracy of the robot, the most direct way is to estimate the environmental parameters online in real time. At time t, ω t Online adjustment and update based on force error A larger σ value will make the system respond faster to force errors, but it may also increase system oscillations. The parameter δ is used to adjust the acceleration term in position control. It multiplies the desired acceleration by To adjust the calculation of the actual acceleration, the damping characteristics of the system can be fine-tuned, affecting the stability and response speed of the system. The force and position conversion expression of the manipulator after introducing adaptive parameters is:

[0053]

[0054] Where T is the system communication cycle between the robot controller and the servo drive, which is a constant value. The acquired position information is sent to the robot to achieve force control at the end.

[0055] In summary, the present invention establishes a load model and zero drift compensation, so that the robot arm can still work stably and efficiently under load. At the same time, adaptive parameters are introduced into the impedance control algorithm, which effectively improves the accuracy and stability of force control.

[0056] It should be noted that, in this article, relational terms such as first and second, etc. are only used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Moreover, the terms "include", "comprise" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, the elements defined by the sentence "comprise a ..." do not exclude the existence of other identical elements in the process, method, article or device including the elements.

[0057] The above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit the same. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that the technical solutions described in the aforementioned embodiments may still be modified, or some of the technical features may be replaced by equivalents. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. An adaptive force control method based on a six-dimensional force sensor at the end of a robotic arm, characterized in that: The steps include: S1. Based on the six-dimensional force sensor, a load compensation model is established to calculate the gravity matrix and zero drift force matrix; S2, based on step S1, compensating the force and torque and establishing a force control model; S3. Based on step S2, adaptive parameters are introduced to convert the contact force with the environment into a motion control signal of the robot arm to achieve constant force control.

2. The adaptive force control method based on the six-dimensional force sensor at the end of the manipulator according to claim 1, characterized in that: Based on step S1, the load model introduction formula is:

3. The adaptive force control method based on the six-dimensional force sensor at the end of the manipulator according to claim 1, characterized in that: Based on step S2, the force and moment formulas are: The combined formula is as follows:

4. The adaptive force control method based on the six-dimensional force sensor at the end of the manipulator according to claim 1, characterized in that: Based on step S1, the feedback force collected by the six-dimensional force sensor is used to compensate gravity and zero drift to obtain the force and torque under the base coordinates. The force collected by the sensor needs to be filtered to optimize the signal.

5. The adaptive force control method based on the six-dimensional force sensor at the end of the manipulator according to claim 4, characterized in that: Based on the force collected by the sensor, the force and torque signals are processed by IIR filter after filtering and optimization, and the transfer function is: The S diagonal matrix is ​​introduced, and the elements on the diagonal are 0 or 1. It is used to constrain the control mode of each degree of freedom in the coordinate system to be position control or force control, and bring the compensated force and torque into impedance control: Where M, B and K represent the inertia matrix, damping matrix and rigidity matrix respectively. and x are the terminal acceleration, terminal velocity and terminal position respectively, and x d are the expected acceleration, expected velocity and expected position, respectively, and F d It is the power of expectation.

6. The adaptive force control method based on the six-dimensional force sensor at the end of the manipulator according to claim 1, characterized in that: Based on step S2, the adaptive parameters σ and δ are introduced in step S3. The force and position conversion expressions of the manipulator after the adaptive parameters are introduced are:

Images