Gravity compensation algorithm, system and equipment for end tool of mechanical arm and storage medium

Abstract

The invention discloses a mechanical arm end tool gravity compensation algorithm, system and device and a storage medium, which are applied to the field of system software, and the method comprises the following steps: obtaining the gravity of a robot end tool; obtaining a zero point value of a force sensor according to the gravity; judging whether the sampling time reaches preset time T or not; if the time T is reached, calculating zero point values within the time T, and taking an average value of the zero point values as a zero point value for starting the force sensor; if not, the zero point value of the force sensor is obtained again; subtracting the gravity and the starting zero point value of the force sensor from the reading of the force sensor to obtain a real external force value; and the mechanical arm moves to execute a corresponding task. According to the method, the mean value within a period of time after the power take-off sensor is started is taken as the zero point value of the starting, the zero point value is calculated once after each time of starting and is taken as the zero point value of the starting, and compared with the prior art that only one fixed zero point value is taken, the method is more accurate.

Description

Technical Field

[0001] The present invention relates to the field of system software, and in particular to a robot arm end tool gravity compensation algorithm, system, device and storage medium. Background Art

[0002] There are many ways for a robot to sense and control the external environment, among which installing a force sensor at the end of the robot is the most common way. In this way, the device is connected between the end of the robot and the force sensor, and the end tool is installed on the force sensor to perform some tasks, such as polishing and grinding. In this installation method, the reading of the force sensor generally includes three parts: the gravity of the end tool, the zero point value of the force sensor itself, and the true value of the contact between the robot and the external environment. We need to obtain the true force value of the contact between the robot and the external environment to accurately control the force of the robot, so we need to consider eliminating the influence of the gravity of the end tool and the zero point of the force sensor on the force sensor reading. The zero drift phenomenon of the force sensor refers to the zero point value of the force sensor being erratic due to the influence of temperature, etc., and its value will change every time it is started, so we need to calibrate the zero point of the force sensor after each start. Zhang Lijian et al. proposed a gravity compensation method, which compensates by calculating the gravity of the end tool and the zero point of the force sensor, but this method ignores the problem of the non-fixed zero point of the force sensor caused by the zero drift of the force sensor.

[0003] Although the prior art compensates by calculating the gravity of the end tool and the zero point of the force sensor, this method ignores the problem of the non-fixed zero point of the force sensor caused by the zero drift of the force sensor, that is, due to the influence of temperature, etc., the zero point value of the force sensor will change each time it is started, but the prior art believes that the zero point of the force sensor is a fixed value each time it is started. The present invention calculates the gravity of the end tool and calibrates the zero point of the force sensor for a period of time after each start of the force sensor, that is, the zero point value of a period of time after the start is averaged as the zero point of this movement. The present invention ensures that the zero point value of the force sensor is related to the environment at the time, and the zero point value is made to better match the current start by taking the average value. Summary of the invention

[0004] The purpose of this application is to provide a robot arm end tool gravity compensation algorithm, system, device and storage medium, aiming to solve the problem of inaccurate readings caused by "zero drift" of sensors in the prior art.

[0005] To achieve the above objectives, this application provides the following technical solutions:

[0006] One of the purposes of this application is to provide a robot end tool gravity compensation algorithm, specifically including:

[0007] A tool gravity compensation algorithm at the end of a robotic arm, wherein a force sensor is installed at the end of the robotic arm, and the algorithm is characterized by comprising:

[0008] Get the gravity of the robot's end tool;

[0009] Obtaining a zero point value of a force sensor according to the gravity;

[0010] Determine whether the sampling time reaches the preset time T;

[0011] If the time T is reached, the zero-point value within the time T is calculated, and the average value of the zero-point value is taken as the zero-point value for starting the force sensor; if not, the zero-point value of the force sensor is obtained again;

[0012] The actual external force value is obtained by subtracting the gravity and the zero value at which the force sensor is started from the reading of the force sensor;

[0013] The robot arm moves to perform the corresponding task.

[0014] Furthermore, the step of obtaining the gravity of the robot end tool specifically includes the following steps:

[0015] Assume the world coordinate system is O0-X0Y0Z0, and let its Z0 axis be vertically upward, which is the opposite direction of gravity. The world coordinate system can be arbitrarily rotated around the direction of gravity.

[0016] The direction vector of the gravity in the world coordinate system O0-X0Y0Z0 is:

[0017]

[0018] Through coordinate transformation, the vector of the gravity in the sensor coordinate system is obtained as:

[0019]

[0020] Assuming the gravity of the robot end tool is G, then:

[0021]

[0022] make:

[0023]

[0024] The values ​​of angles U and V are:

[0025] The gravity of the robot end tool

[0026] Furthermore, the reading of the force sensor is recorded as: [Fx F y F z M x M y M z ] T , where F x F y F z are the three force components of the sensor, M x M y M z are the three torque components of the sensor;

[0027] The zero point value of the force sensor is recorded as: [F 0x F 0y F 0z M 0x M 0y M 0z ] T ;

[0028] The external force value is the actual force value of the contact between the robot arm and the outside world, which is recorded as: [F ex F ey F ez M ex M ey M ez ] T ;

[0029] The gravity of the robot end tool will change with the change of posture, which is recorded as: [F Gx F Gy F Gz M Gx M Gy M Gz ] T ;

[0030] The relationship between the gravity, the reading of the force sensor, the zero value of the force sensor and the external force value is:

[0031]

[0032] Furthermore, the step of obtaining the zero point value of the force sensor according to the gravity specifically includes the following steps:

[0033] Within the preset time T, the force sensor remains stationary after being activated and is not acted upon by external force. At this time, the calculation method of the zero point value of the force sensor is:

[0034]

[0035] Where F(t) and M(t) are the force and torque values ​​of the force sensor at time t during the operation of the robot arm, F G (t) and M G (t) is the force and torque value of the end tool in the force sensor coordinate system at time t.

[0036] Furthermore, the step of taking the average value of the zero-point value as the zero-point value for starting the force sensor specifically includes the following steps:

[0037] The average value of the zero-point value within time T is taken as the zero-point value of the sensor startup. The calculation formula is:

[0038]

[0039] Among them, k is the number of points collected by the force sensor within the time T; that is, the zero point value is calculated for each time T, and the average value of all zero point values ​​within the time T is calculated after being aggregated, and the average value is used as the zero point value for sensor startup.

[0040] Furthermore, the step of obtaining the real external force value by subtracting the gravity and the zero value of the force sensor from the reading of the force sensor comprises the following steps:

[0041] The actual external force value is obtained by subtracting the gravity and the zero value of the force sensor from the reading of the force sensor. The formula is:

[0042]

[0043] The second object of the present application is to provide a robot end tool gravity compensation system, comprising:

[0044] Acquisition module: obtains the gravity of the robot's end tool;

[0045] Calculation module: obtaining the zero point value of the force sensor according to the gravity;

[0046] Judgment module: judge whether the sampling time reaches the preset time T;

[0047] Judgment result module: if the time T is reached, the zero point value within the time T is calculated, and the average value of the zero point value is taken as the zero point value for starting the force sensor; if not, the zero point value of the force sensor is re-obtained;

[0048] Output module: obtains the real external force value by subtracting the gravity and the zero value of the force sensor from the reading of the force sensor;

[0049] Execution module: The robotic arm moves to perform the corresponding tasks.

[0050] The third object of the present application is to provide a device, the device comprising a processor and a memory coupled to the processor, wherein:

[0051] The memory stores program instructions for implementing the robot arm end tool gravity compensation algorithm;

[0052] The processor is used to execute the program instructions stored in the memory to provide a robot end-of-arm tool gravity compensation algorithm.

[0053] A fourth object of the present application is to provide a storage medium storing program instructions executable by a processor, wherein the program instructions are used to execute the robot arm end tool gravity compensation algorithm.

[0054] The present application provides a robot end tool gravity compensation algorithm, system, device and storage medium, which have the following beneficial effects:

[0055] The solution proposed in this application calculates the zero value of the force sensor more accurately, making the measured external force more accurate and the mechanical arm force control effect more effective. This method takes the average value within a period of time after the force sensor is started as the zero value of this startup, and calculates the zero value once after each startup as the zero value of this startup. Compared with the prior art that only takes a fixed zero value, this method is more accurate. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 This is a flowchart of a gravity compensation algorithm for a tool at the end of a robot arm according to Embodiment 1 of the present application;

[0057] Figure 2 This is a schematic structural diagram of a robot arm end tool gravity compensation system according to Embodiment 2 of the present application;

[0058] Figure 3 A schematic diagram of the device structure provided in Example 3 of the present invention;

[0059] Figure 4 A schematic diagram of the storage medium structure provided in Example 4 of the present invention. DETAILED DESCRIPTION

[0060] The embodiments of the present application are described in detail below, and examples of the embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are intended to be used to explain the present application, and should not be construed as limiting the present application.

[0061] In the description of the present application, it should be understood that the terms "upper", "lower", "horizontal", "inside", "outside", etc., indicating orientations or positional relationships, are based on the orientations or positional relationships shown in the accompanying drawings, and are only for the convenience of describing the present application and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore should not be understood as a limitation on the present application.

[0062] In addition, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of the features. In the description of this application, the meaning of "plurality" is two or more, unless otherwise clearly and specifically defined.

[0063] In order to make the objectives, technical solutions and advantages of the present application more clearly understood, the present application is further described in detail below in conjunction with the accompanying drawings and embodiments.

[0064] Example 1

[0065] Reference Figure 1 , which is a flow chart of a robot arm end tool gravity compensation algorithm proposed in this application, including the following steps S1 to S6, and the implementation method of each step is described in detail below.

[0066] The present application provides a robot end tool gravity compensation algorithm, the steps of which include:

[0067] Step S1: Obtain the gravity of the robot end tool.

[0068] In this embodiment, step S1 specifically includes the following steps:

[0069] Step S11: Set the world coordinate system to O0-X0Y0Z0, and let the Z0 axis be vertically upward, which is the opposite direction of gravity. The world coordinate system can be arbitrarily rotated around the direction of gravity;

[0070] Step S12: The direction vector of the gravity in the world coordinate system O0-X0Y0Z0 is:

[0071]

[0072] Step S13: Through coordinate transformation, the vector of the gravity in the sensor coordinate system is obtained as:

[0073]

[0074] Step S14: Assuming the gravity of the robot end tool is G, then:

[0075]

[0076] make:

[0077]

[0078] The values ​​of angles U and V are:

[0079] The gravity of the robot end tool

[0080] Step S2: Obtaining a zero point value of the force sensor according to the gravity.

[0081] In this embodiment, step S2 specifically includes the following steps:

[0082] Within the preset time T, the force sensor remains stationary after being activated and is not acted upon by external force. At this time, the calculation method of the zero point value of the force sensor is:

[0083]

[0084] Where F(t) and M(t) are the force and torque values ​​of the force sensor at time t during the operation of the robot arm, F G (t) and M G (t) is the force and torque value of the end tool in the force sensor coordinate system at time t.

[0085] Step S3: Determine whether the sampling time reaches the preset time T.

[0086] Step S4: If the time T is reached, the zero-point value within the time T is calculated, and the average value of the zero-point value is taken as the zero-point value for starting the force sensor; if not, the zero-point value of the force sensor is re-acquired.

[0087] In this embodiment, step S4 specifically includes the following steps:

[0088] The average value of the zero-point value within time T is taken as the zero-point value of the sensor startup. The calculation formula is:

[0089]

[0090] Among them, k is the number of points collected by the force sensor within the time T; that is, the zero point value is calculated for each time T, and the average value of all zero point values ​​within the time T is calculated after being aggregated, and the average value is used as the zero point value for sensor startup.

[0091] Step S5: The real external force value is obtained by subtracting the gravity and the zero value at which the force sensor is started from the reading of the force sensor.

[0092] In this embodiment, step S5 specifically includes the following steps:

[0093] The actual external force value is obtained by subtracting the gravity and the zero value of the force sensor from the reading of the force sensor. The formula is:

[0094]

[0095] The external force value is the actual force value of the contact between the robot arm and the outside world, which is recorded as: [F ex F ey F ez M ex M ey M ez ] T .

[0096] The relationship between the gravity, the reading of the force sensor, the zero value of the force sensor and the external force value is:

[0097]

[0098] The reading of the force sensor is recorded as: [F x F y F z M x M y M z ] T , where F x F y F z are the three force components of the sensor, M x M y M z are the three torque components of the sensor;

[0099] The zero point value of the force sensor is recorded as: [F 0x F 0y F 0z M 0x M 0y M 0z ] T ;

[0100] The gravity of the robot end tool will change with the change of posture, which is recorded as: [F Gx F Gy F Gz M Gx M Gy M Gz ] T .

[0101] Step S6: The robot arm moves to perform the corresponding task.

[0102] To summarize, Example 1 of the present application calculates the zero point value of the force sensor, so that the measured external force is more accurate and the robotic arm force control effect is more effective; this method calculates the zero point value within a period of time after the force sensor is started, and then takes the average value as the zero point value of this force sensor startup, and calculates the zero point value once after each startup as the zero point value of this force sensor startup, which is more accurate than the prior art of only taking a fixed zero point value.

[0103] Example 2

[0104] Reference Figure 2 , which is a structural schematic diagram of a robot arm end tool gravity compensation system proposed in this application;

[0105] The present application provides a robot arm end tool gravity compensation system, the specific contents of which include:

[0106] Acquisition module 100: Acquisition of gravity of the robot end tool;

[0107] Calculation module 200: obtaining a zero point value of a force sensor according to the gravity;

[0108] Determination module 300: Determine whether the sampling time reaches the preset time T;

[0109] Determine result module 400: if the time T is reached, calculate the zero point value within the time T, and take the average value of the zero point value as the zero point value for starting the force sensor; if not, re-acquire the zero point value of the force sensor;

[0110] Output module 500: obtains a real external force value by subtracting the gravity and the zero value of the force sensor from the reading of the force sensor;

[0111] Execution module 600: The robot arm moves to execute the corresponding task.

[0112] In this embodiment, first, the robot and the force sensor are powered on respectively, and then the acquisition module 100 calculates the gravity of the robot's end tool by a gravity compensation method, and the calculation module 200 calculates the zero point value of the force sensor; when the judgment module 300 determines that the sampling time reaches the preset time T, the zero point value within the time T is calculated, and the average value is taken as the zero point value of the force sensor startup this time; the real external force value can be obtained by subtracting the gravity and the zero point value of the force sensor startup from the reading of the force sensor, and finally the execution module 600 executes the corresponding task through the movement of the robotic arm.

[0113] In summary, Example 2 of the present application proposes a gravity compensation system for the end tool of a robot arm, aiming at the influence of the end tool of the robot arm on the reading of the force sensor, so as to achieve precise force control of the end of the robot arm; first, the gravity of the end tool is calculated by the gravity compensation method, and then the zero point value is calculated by the online calibration method, and the average value is taken as the zero point value of the force sensor for this startup, and finally the real external force value is calculated, which ensures that the zero point value of the force sensor is related to the environment at that time, and the zero point value is made to better match the current startup by taking the average value.

[0114] Example 3

[0115] Reference Figure 3 , is a schematic diagram of the device structure of an embodiment of the present application. The device 50 includes a processor 51 and a memory 52 coupled to the processor 51.

[0116] The memory 52 stores program instructions for implementing the above-mentioned three-network information fusion and retrieval system.

[0117] The processor 51 is used to execute the program instructions stored in the memory 52 to realize the fusion and retrieval of the three-network information.

[0118] The processor 51 may also be referred to as a CPU (Central Processing Unit). The processor 51 may be an integrated circuit chip having the ability to process signals. The processor 51 may also be a general-purpose processor, a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field-programmable gate array (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components. The general-purpose processor may be a microprocessor or the processor may also be any conventional processor, etc.

[0119] Example 4

[0120] Reference Figure 4 , which is a schematic diagram of the structure of the storage medium of the embodiment of the present application. The storage medium of the embodiment of the present application stores a program file 61 that can implement all the above methods, wherein the program file 61 can be stored in the above storage medium in the form of a software product, including a number of instructions to enable a computer device (which can be a personal computer, a server, or a network device, etc.) or a processor (processor) to perform all or part of the steps of the methods of each embodiment of the present invention. The aforementioned storage medium includes: various media that can store program codes, such as a USB flash drive, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk, or a computer, a server, a mobile phone, a tablet and other devices.

[0121] The serial numbers of the above embodiments of the present invention are only for description and do not represent the advantages or disadvantages of the embodiments.

[0122] In the above embodiments of the present invention, the description of each embodiment has its own emphasis. For parts that are not described in detail in a certain embodiment, reference can be made to the relevant descriptions of other embodiments.

[0123] In the several embodiments provided in this application, it should be understood that the disclosed technical content can be implemented in other ways. Among them, the system embodiments described above are only schematic. For example, the division of units can be a logical function division. There may be other division methods in actual implementation. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of units or modules, which can be electrical or other forms.

[0124] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed over multiple units. Some or all of the units may be selected according to actual needs to achieve the purpose of the present embodiment.

[0125] In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0126] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including a number of instructions for a computer device (which can be a personal computer, a server or a network device, etc.) to perform all or part of the steps of the methods of each embodiment of the present invention. The aforementioned storage medium includes: U disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), mobile hard disk, magnetic disk or optical disk and other media that can store program codes.

[0127] It can be understood that the technical features of the above-described embodiments can be arbitrarily combined. In order to make the description concise, not all possible combinations of the technical features in the above-described embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0128] The above are only preferred embodiments of the present application, and only specifically describe the technical principles of the present application. These descriptions are only for explaining the principles of the present application and cannot be interpreted as limiting the scope of protection of the present application in any way. Based on the explanation here, any modifications, equivalent substitutions and improvements made within the spirit and principles of the present application, and other specific implementation methods of the present application that can be associated with the technicians in this field without creative work, should be included in the scope of protection of the present application.

Claims

1. A tool gravity compensation algorithm at the end of a robotic arm, wherein a force sensor is installed at the end of the robotic arm, characterized in that: include: Get the gravity of the robot's end tool; Obtaining a zero point value of a force sensor according to the gravity; Determine whether the sampling time reaches the preset time T; If the time T is reached, the zero-point value within the time T is calculated, and the average value of the zero-point value is taken as the zero-point value for starting the force sensor; if not, the zero-point value of the force sensor is obtained again; The actual external force value is obtained by subtracting the gravity and the zero value at which the force sensor is started from the reading of the force sensor; The robot arm moves to perform the corresponding task.

2. The robot arm end tool gravity compensation algorithm according to claim 1, characterized in that: The step of obtaining the gravity of the robot end tool specifically includes the following steps: Assume the world coordinate system is O0-X0Y0Z0, and let its Z0 axis be vertically upward, which is the opposite direction of gravity. The world coordinate system can be arbitrarily rotated around the direction of gravity. The direction vector of the gravity in the world coordinate system O0-X0Y0z0 is: Through coordinate transformation, the vector of the gravity in the sensor coordinate system is obtained as: Assuming the gravity of the robot end tool is G, then: make: The values ​​of angles U and V are: The gravity of the robot end tool 3. A robot arm end tool gravity compensation algorithm according to claim 1 or 2, characterized in that: The reading of the force sensor is recorded as: [F x F y F z M x M y M z ] T , where F x F y F z are the three force components of the sensor, M x M y M z are the three torque components of the sensor; The zero point value of the force sensor is recorded as: [F 0x F 0y F 0z M 0x M 0y M 0z ] T ; The external force value is the actual force value of the contact between the robot arm and the outside world, which is recorded as: [F ex F ey F ez M ex M ey M ez ] T ; The gravity of the robot end tool will change with the change of posture, which is recorded as: [F Gx F Gy F Gz M Gx M Gy M Gz ] T ; The relationship between the gravity, the reading of the force sensor, the zero value of the force sensor and the external force value is:

4. The robot end tool gravity compensation algorithm based on online calibration of force sensor zero point according to claim 1, characterized in that: The step of obtaining the zero point value of the force sensor according to the gravity specifically includes the following steps: Within the preset time T, the force sensor remains stationary after being activated and is not acted upon by external force. At this time, the calculation method of the zero point value of the force sensor is: Where F(t) and M(t) are the force and torque values ​​of the force sensor at time t during the operation of the robot arm, F G (t) and M G (t) is the force and torque value of the end tool in the force sensor coordinate system at time t.

5. The robot arm end tool gravity compensation algorithm according to claim 1, characterized in that: The step of taking the average value of the zero-point value as the zero-point value for starting the force sensor specifically comprises the following steps: The average value of the zero-point value within time T is taken as the zero-point value of the sensor startup. The calculation formula is: Among them, k is the number of points collected by the force sensor within the time T; that is, the zero point value is calculated for each time T, and the average value of all zero point values ​​within the time T is calculated after being aggregated, and the average value is used as the zero point value for sensor startup.

6. The robot arm end tool gravity compensation algorithm according to claim 1, characterized in that: The step of obtaining the real external force value by subtracting the gravity and the zero value of the force sensor from the reading of the force sensor comprises the following steps: The actual external force value is obtained by subtracting the gravity and the zero value of the force sensor from the reading of the force sensor. The formula is:

7. A system for a robot end tool gravity compensation algorithm according to claim 1, characterized in that: include: Acquisition module: obtains the gravity of the robot's end tool; Calculation module: obtaining the zero point value of the force sensor according to the gravity; Judgment module: judge whether the sampling time reaches the preset time T; Judgment result module: if the time T is reached, the zero point value within the time T is calculated, and the average value of the zero point value is taken as the zero point value for starting the force sensor; if not, the zero point value of the force sensor is re-obtained; Output module: obtains the real external force value by subtracting the gravity and the zero value of the force sensor from the reading of the force sensor; Execution module: The robotic arm moves to perform the corresponding tasks.

8. A device, characterized in that The device comprises a processor and a memory coupled to the processor, wherein: The memory stores program instructions for implementing a robot end tool gravity compensation algorithm as described in any one of claims 1 to 6; The processor is used to execute the program instructions stored in the memory to provide a robot end-of-arm tool gravity compensation algorithm.

9. A storage medium, characterized in that: Processor-executable program instructions are stored, and the program instructions are used to execute a robot arm end tool gravity compensation algorithm as described in any one of claims 1-6.

Images