Gravity compensation algorithm for tail end tool of robotic arm, and system, device and storage medium

Abstract

Disclosed in the present invention are a gravity compensation algorithm for a tail end tool of a robotic arm, and a system, a device and a storage medium, which are applied to the field of system software. The method comprises: acquiring the gravity of a tail end tool of a robot; acquiring zero-point values of a force sensor according to the gravity; determining whether a sampling time reaches a preset time T; if the sampling time reaches the time T, calculating zero-point values within the time T, and taking the average value of the zero-point values as a zero-point value at the start of the force sensor; and if the sampling time does not reach the time T, re-acquiring zero-point values of the force sensor; subtracting the gravity and the zero-point value at the start of the force sensor from a reading of the force sensor, so as to obtain a real external force value; and a robotic arm moving to execute a corresponding task. In the method, the average value of zero-point values within a time period after the start of a force sensor is used as a zero-point value at the current start, and a zero-point value is calculated once after each start and is used as the zero-point value at the current start. Therefore, compared to the prior art in which only one fixed zero-point value is taken, the method is more accurate.

Description

A robot arm end tool gravity compensation algorithm, system, device and storage medium 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 to sense and control the force of a robotic arm's external environment. Mounting a force sensor at the end of the arm is the most common method. In this method, the device is connected to the end of the arm and the force sensor using a mechanical connection. An end-of-line tool is also mounted on the force sensor to perform tasks such as polishing or grinding. With this installation method, the force sensor reading generally consists of three components: the weight of the end-of-line tool, the zero point of the force sensor itself, and the actual value of the robot's contact with the external environment. To accurately control the robot's force, we need to eliminate the effects of the weight of the end-of-line tool and the zero point of the force sensor on the force sensor reading. Force sensor zero drift refers to the phenomenon where the zero point of the force sensor fluctuates due to temperature and other factors, and the value changes with each startup. Therefore, zero point calibration is required after each startup. Zhang Lijian et al. proposed a gravity compensation method that calculates the weight of the end-of-line tool and the zero point of the force sensor. However, this method ignores the issue of force sensor zero drift, which can cause the zero point of the force sensor to fluctuate.

[0003] While existing technologies compensate by calculating the end-tool's gravity and the force sensor's zero point, this approach ignores the issue of the force sensor's unstable zero point, caused by zero drift. This is because the zero point value of the force sensor changes each time it's activated due to factors such as temperature, yet existing technologies assume that the zero point of the force sensor is fixed at each activation. The present invention calculates the end-tool's gravity and then calibrates the force sensor's zero point for a period of time after each activation. Specifically, the zero point values ​​over this period are averaged as the zero point for that movement. This ensures that the force sensor's zero point value is consistent with the prevailing environment, and by taking the average, the zero point value more closely matches the current activation. Technical issues

[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. Technical Solutions

[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 gravity compensation algorithm for a tool at the end of a robotic arm, specifically including:

[0007] A tool gravity compensation algorithm for a robotic arm end-mounted tool, wherein a force sensor is installed at the end of the robotic arm, 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 values ​​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] Obtain a true external force value by subtracting the gravity and the zero point value of the force sensor from the reading of the force sensor;

[0013] The robotic 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 , so that The axis direction is vertically upward, which is the opposite direction of gravity. The world coordinate system can be arbitrarily rotated around the direction of gravity.

[0016] The gravity in the world coordinate system The direction vector in 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: ,in are the three force components of the sensor, are the three torque components of the sensor;

[0027] The zero point value of the force sensor is recorded as: ;

[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: ;

[0029] The gravity of the robot end tool will change with the change of posture, which can be expressed as: ;

[0030] The relationship between the gravity, the force sensor reading, the force sensor zero value 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] During the preset time T, the force sensor remains stationary after being activated and is not affected by external force. The calculation method of the zero point value of the force sensor at this time 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. and are the force and torque values ​​of the end tool in the force sensor coordinate system at time t.

[0036] Furthermore, the step of using the average value of the zero-point values ​​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 used as the zero-point value of the sensor startup. The calculation formula is:

[0038] ;

[0039] Here, 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 activation.

[0040] Furthermore, the step of obtaining the actual 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] A second object of the present application is to provide a robot end-of-arm tool gravity compensation system, comprising:

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

[0045] Calculation module: obtains 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, calculate the zero-point value within the time T, and take the average value of the zero-point values ​​as the zero-point value for starting the force sensor; if not, re-acquire the zero-point value of the force sensor;

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

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

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

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

[0052] The processor is configured 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 aforementioned robot arm end tool gravity compensation algorithm. Beneficial effects

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

[0055] The proposed solution more accurately calculates the zero-point value of the force sensor, resulting in more precise external force measurements and more effective robotic arm force control. This method uses the average value of the force sensor over a period of time after activation as the zero-point value for that activation, and calculates the zero-point value once after each activation. This method is more accurate than existing technologies that use only a fixed zero-point value. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] FIG1 is a flowchart of a gravity compensation algorithm for an end-of-arm tool according to Example 1 of the present application;

[0057] FIG2 is a schematic structural diagram of a gravity compensation system for a robot arm end tool according to Example 2 of the present application;

[0058] FIG3 is a schematic diagram of the structure of a device provided in Example 3 of the present invention;

[0059] FIG4 is a schematic diagram of the storage medium structure provided by Embodiment 4 of the present invention. Best Mode for Carrying Out the Invention

[0060] The following describes in detail embodiments of the present application, examples of which 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 this 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 this 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 this application.

[0062] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of the technical features being referred to. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of the features. Throughout the description of this application, "plurality" means two or more, unless otherwise specifically defined.

[0063] In order to make the purpose, technical solutions and advantages of this application more clear, this application is further described in detail below with reference to the accompanying drawings and embodiments.

[0064] Example 1

[0065] 1 , which is a flow chart of a gravity compensation algorithm for an end-of-arm tool proposed in this application, includes the following steps S1 to S6 , and the implementation of each step is described in detail below.

[0066] This application provides a robot arm end tool gravity compensation algorithm, the steps 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 , so that The axis direction is 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 gravity is in the world coordinate system The direction vector in 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] During the preset time T, the force sensor remains stationary after being activated and is not affected by external force. The calculation method of the zero point value of the force sensor at this time 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. and are the force and torque values ​​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-obtained.

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

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

[0089] ;

[0090] Here, 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 activation.

[0091] Step S5: Subtract the gravity and the zero point value of the force sensor from the reading of the force sensor to obtain the actual external force value.

[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: .

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

[0097] .

[0098] The reading of the force sensor is recorded as: ,in are the three force components of the sensor, are the three torque components of the sensor;

[0099] The zero point value of the force sensor is recorded as: ;

[0100] The gravity of the robot end tool will change with the change of posture, which can be expressed as: .

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

[0102] To sum up, 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 existing technology of only taking a fixed zero-point value. Modes for Carrying Out the Invention

[0103] Example 2

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

[0105] This application provides a robot arm end tool gravity compensation system, specifically including:

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

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

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

[0109] Determination result module 400: If the time T is reached, calculate the zero-point value within the time T, and take the average of the zero-point values ​​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 point value of the force sensor from the reading of the force sensor;

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

[0112] In this embodiment, the robot and the force sensor are first 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] To summarize, Example 2 of the present application addresses the impact of the end tool of the robotic arm on the force sensor readings and proposes a gravity compensation system for the end tool of the robotic arm to achieve precise force control of the end of the robotic 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, ensuring that the zero point value of the force sensor is related to the environment at the time, and the zero point value is made more compatible with this startup by taking the average value.

[0114] Example 3

[0115] 3 , which is a schematic diagram of a device structure according to an embodiment of the present application, shows that 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 program instructions stored in the memory 52 to achieve three-network information fusion and retrieval.

[0118] The processor 51 may also be referred to as a CPU (Central Processing Unit). The processor 51 may be an integrated circuit chip with signal processing capabilities. 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 device, a discrete gate or transistor logic device, or a discrete hardware component. The general-purpose processor may be a microprocessor or any conventional processor.

[0119] Example 4

[0120] Referring to FIG4 , which is a schematic diagram of the structure of the storage medium of an embodiment of the present application. The storage medium of the embodiment of the present application stores a program file 61 that can implement all of 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 for causing a computer device (which can be a personal computer, server, or network device, etc.) or a processor to execute 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, server, mobile phone, tablet, etc., or a device such as a tablet.

[0121] The serial numbers of the above embodiments of the present invention are for description only 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 focus. 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 exemplary. For example, the division of units can be a logical function division. In actual implementation, there may be other division methods, such as 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] Units described as separate components may or may not be physically separate, and components shown as units may or may not be physical units, that is, they may be located in one place or distributed across multiple units. Some or all of the units may be selected to achieve the purpose of the present embodiment according to actual needs.

[0125] In addition, the functional units in the various embodiments of the present invention may be integrated into a single processing unit, each unit may exist physically separately, or two or more units may be integrated into a single unit. The aforementioned integrated units may be implemented in the form of hardware or 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, 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. The computer software product is stored in a storage medium and includes several instructions for enabling a computer device (which can be a personal computer, server or network device, etc.) to execute 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, etc. Various media that can store program codes.

[0127] It can be understood that the various technical features of the above-described embodiments can be combined arbitrarily. In order to make the description concise, not all possible combinations of the various 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 merely preferred embodiments of the present application and only specifically describe the technical principles of the present application. These descriptions are intended only to explain the principles of the present application and should not be construed in any way as limiting the scope of protection of the present application. Based on the explanations herein, any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present application, as well as other specific implementations of the present application that can be conceived by those skilled in the art without inventive effort, shall be included within 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 real 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 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 , so that The axis direction is vertically upward, which is the opposite direction of gravity. The world coordinate system can be arbitrarily rotated around the direction of gravity. The gravity is in the world coordinate system The direction vector in 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: ,in are the three force components of the sensor, are the three torque components of the sensor; The zero point value of the force sensor is recorded as: ； The external force value is the actual force value of the contact between the robot arm and the outside world, which is expressed as: ； The gravity of the robot end tool will change with the change of posture, which is recorded as: ； The relationship between the gravity, the reading of the force sensor, the zero point 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: ； in 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. and are the force and torque values ​​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