Virtual torque sensor of joint servo module based on multi-sensor fusion technology

Abstract

The present invention belongs to the technical field of joint servo modules, and specifically relates to a virtual torque sensor of a joint servo module based on multi-sensor fusion technology, comprising a calculation module and a first rotation angle meter and a second rotation angle meter installed at the input and output ends of a harmonic reducer; the present invention uses the output rotation difference of the first rotation angle meter and the second rotation angle meter on a servo module based on a harmonic reducer to characterize the characteristics of the joint torque to a certain extent, and then uses multi-sensor fusion technology combined with an artificial neuron learning algorithm to finally build a virtual torque sensor. The virtual torque sensor of the present invention does not require the additional installation of a torque sensor, and has the characteristics of low cost, high system reliability, accurate torque calculation, and high dynamic response.

Description

Technical Field

[0001] The present invention belongs to the technical field of joint servo modules, and in particular relates to a virtual torque sensor of a joint servo module based on multi-sensor fusion technology. Background Art

[0002] The robot arm joint servo system usually consists of a servo joint module consisting of a servo motor, an input end rotation angle meter, an output end rotation angle meter, a harmonic reducer, etc.

[0003] During the operation of the joint servo module, in order to monitor the size of the robot arm joint torque, there are usually two ways to obtain the size of the joint torque. One is to calculate it indirectly based on the current and torque constant of the servo motor; the other is to install a torque sensor in the servo module to directly detect the torque size. The former calculation of the torque size is based on the servo motor control theory and cannot truly and accurately reflect the torque changes and response conditions of the actual system. The latter can detect the joint torque in real time, but requires additional installation space and brings about the problem of increased system cost. Summary of the Invention

[0004] In view of this, the present invention proposes a virtual torque sensor for a joint servo module based on multi-sensor fusion technology. The present invention uses the output rotation angle difference of the first rotation angle meter and the second rotation angle meter on a servo module based on a harmonic reducer to characterize the characteristics of the joint torque to a certain extent, and uses a multi-sensor data fusion technology algorithm based on artificial neurons to obtain the real-time torque mathematical relationship corresponding to the rotation angle difference, and then calculate the joint torque in real time. The virtual torque sensor constructed by the present invention has the characteristics of low cost, high system reliability, accurate torque calculation and high dynamic response.

[0005] In order to achieve the above technical objectives, the specific technical solutions adopted by the present invention are:

[0006] A virtual torque sensor of a joint servo module based on multi-sensor fusion technology is used for torque measurement of the joint servo module. The joint servo module realizes motor deceleration based on a harmonic reducer with flexible transmission. The virtual torque sensor includes:

[0007] A first rotation measuring device is installed at the input end of the harmonic reducer and is used to measure the motor rotor angle data at the input end of the harmonic reducer;

[0008] a second rotation measuring device, installed at the output end of the harmonic reducer, for measuring the rotation angle data of the output end of the harmonic reducer;

[0009] a calculation module having a built-in neuron learning algorithm, communicating with the first rotation measuring device and the second rotation measuring device, and configured to collect a rotation difference between the first rotation measuring device and the second rotation measuring device;

[0010] Wherein: the measured torque is calculated based on the rotation difference, based on the torque data of the torque observer of the joint servo module, and based on the current data of the motor;

[0011] The neuron learning algorithm is configured to collect each of the rotation difference values, and then perform learning calculations based on the multi-sensor fusion technology to combine the rotation difference values ​​with the measured torque to obtain the precise torque of the harmonic reducer.

[0012] Furthermore, the method for deriving the measured torque based on the rotation difference is implemented by the following formula:

[0013]

[0014] Where:

[0015] R: Gear reduction ratio of joint servo module / harmonic reducer;

[0016] T: measured torque;

[0017] θ in : Input angle of joint servo module / harmonic reducer;

[0018] θ out : Output angle of joint servo module / harmonic reducer;

[0019] Ks: Stiffness coefficient of joint servo module / harmonic reducer.

[0020] Furthermore, the method for obtaining the measured torque based on the torque coefficient of the motor is implemented by the following formula:

[0021] T e =i qcmd *k t

[0022] Where:

[0023] Te: electromagnetic torque of the motor;

[0024] i qcmd : Current command on the motor q axis;

[0025] Kt: Torque coefficient of the motor.

[0026] Furthermore, the computing module includes an algorithm chip, a cache unit, and a data storage unit;

[0027] The algorithm chip is embedded with the logic of the neuron learning algorithm;

[0028] The cache unit exchanges data with the algorithm chip to cooperate with the neuron computing algorithm chip to execute the neuron learning algorithm.

[0029] Furthermore, the computing module also includes a data storage unit; the data storage unit interacts with the algorithm chip to store operating data of the algorithm chip.

[0030] Furthermore, the calculation module communicates with the control chip of the motor and the torque observer to collect the current command on the q-axis of the motor and the torque data of the torque observer.

[0031] Furthermore, the algorithm chip is also used to feed back the precise torque to the control chip; the control chip controls the motor with reference to the precise torque compensation.

[0032] Furthermore, the first rotation measuring device and the second rotation measuring device are both rotation angle measuring devices; the rotation difference value is the angle difference calculated by substituting the measured angle difference of the first rotation measuring device and the second rotation measuring device into the gear reduction ratio of the harmonic reducer. BRIEF DESCRIPTION OF THE DRAWINGS

[0033] In order to more clearly illustrate the technical solutions of the embodiments of the present disclosure, the following briefly introduces the drawings required for use in the embodiments. Obviously, the drawings described below are only some embodiments of the present disclosure. For ordinary technicians in this field, other drawings can be obtained based on these drawings without any creative work.

[0034] Figure 1 Schematic diagram of the installation of the first rotation measuring device and the second rotation measuring device in a specific embodiment of the present invention;

[0035] Figure 2 is the angle difference θ between the two rotation angle measuring devices in the specific embodiment of the present invention T Schematic diagram of the relationship between it and the module torque Te;

[0036] Figure 3 This is a schematic diagram of a mathematical relationship for calculating the measured torque based on the stiffness coefficient in a specific embodiment of the present invention;

[0037] Figure 4 A schematic diagram of a mathematical relationship for calculating the measured torque based on the torque coefficient in a specific embodiment of the present invention;

[0038] Figure 5 This is a schematic diagram of the mathematical relationship of multi-sensor fusion calculation in a specific embodiment of the present invention;

[0039] Figure 6 This is a schematic diagram of the principle of performing multi-path learning iterative calculation of mathematical relationships after normalization processing in a specific embodiment of the present invention;

[0040] Figure 7 A schematic diagram of the hardware composition of the servo driver of the joint servo module in a specific embodiment of the present invention;

[0041] Figure 8 This is a schematic diagram of the principle of obtaining the measured torque by the torque observer in a specific embodiment of the present invention;

[0042] Figure 9 A schematic diagram of a mathematical relationship for calculating the measured torque based on the torque coefficient and the torque observer in a specific embodiment of the present invention;

[0043] Among them: 1. First rotation measuring device; 2. Second rotation measuring device; 3. Harmonic reducer. DETAILED DESCRIPTION

[0044] The embodiments of the present disclosure are described in detail below with reference to the accompanying drawings.

[0045] The following describes the embodiments of the present disclosure through specific examples, and those skilled in the art can easily understand other advantages and effects of the present disclosure from the contents disclosed in this specification. Obviously, the described embodiments are only a part of the embodiments of the present disclosure, rather than all of the embodiments. The present disclosure can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed in various ways based on different viewpoints and applications without departing from the spirit of the present disclosure. It should be noted that, in the absence of conflict, the following embodiments and features in the embodiments can be combined with each other. Based on the embodiments in the present disclosure, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present disclosure.

[0046] It should be noted that various aspects of the embodiments within the scope of the appended claims are described below. It should be apparent that the aspects described herein can be embodied in a wide variety of forms, and any specific structure and / or function described herein is merely illustrative. Based on this disclosure, it should be understood by those skilled in the art that an aspect described herein can be implemented independently of any other aspect, and two or more of these aspects can be combined in various ways. For example, any number of aspects described herein can be used to implement an apparatus and / or practice a method. In addition, other structures and / or functionalities other than one or more of the aspects described herein can be used to implement this apparatus and / or practice this method.

[0047] It should also be noted that the illustrations provided in the following embodiments are only schematic illustrations of the basic concept of the present disclosure. The illustrations only show components related to the present disclosure and are not drawn according to the number, shape and size of components in actual implementation. In actual implementation, the type, quantity and proportion of each component can be changed at will, and the component layout type may also be more complicated.

[0048] Additionally, in the following description, specific details are provided to provide a thorough understanding of the examples. However, one skilled in the art will appreciate that the aspects described can be practiced without these specific details.

[0049] In one embodiment of the present invention, a virtual torque sensor of a joint servo module based on multi-sensor fusion technology is proposed for torque measurement of the joint servo module. The joint servo module realizes motor deceleration based on a harmonic reducer 3 of flexible transmission; the virtual torque sensor includes:

[0050] The first rotation measuring device 1 is installed at the input end of the harmonic reducer 3 and is used to measure the rotation data of the input end of the harmonic reducer 3;

[0051] The second rotation measuring device 2 is installed at the output end of the harmonic reducer 3 and is used to measure the rotation data of the output end of the harmonic reducer 3;

[0052] a calculation module having a built-in neuron learning algorithm, communicating with the first rotation measuring device 1 and the second rotation measuring device 2, and used for collecting the rotation difference between the first rotation measuring device 1 and the second rotation measuring device 2;

[0053] Wherein: the measured torque is calculated based on the rotation difference, based on the torque data of the torque observer of the joint servo module, and based on the current data of the motor;

[0054] The neuron learning algorithm is configured to collect each of the rotation difference values, and then perform learning calculations based on the multi-sensor fusion technology to combine the rotation difference values ​​with the measured torque to obtain the precise torque of the harmonic reducer 3.

[0055] In this embodiment, the harmonic reducer 3 is a flexible transmission, such as Figure 1 As shown, in this embodiment, the rotation angle difference obtained by the output difference between the first rotation measuring device 1 and the second rotation measuring device 2 on the joint servo module based on the harmonic reducer 3 can characterize the characteristics of the joint torque of the joint servo module to a certain extent. At the same time, the measured torque obtained by other devices on the original joint servo module is introduced, and the multi-sensor data fusion technology algorithm based on the neuron learning algorithm is used to calculate the mathematical relationship between the rotation angle difference and the torque in real time, so as to obtain a more accurate and reliable joint torque measurement value compared with the existing technology.

[0056] This embodiment utilizes a neural learning algorithm to achieve relatively accurate torque measurement. Specifically, data from the harmonic reducer 3 is collected and a corresponding coordinate system is established to learn the measured torque reflected by the harmonic reducer. This prevents the influence of sudden data points on the rotational difference-torque relationship and thus obtains a relatively accurate rotational difference-torque relationship. Subsequently, a multi-sensor data fusion algorithm based on the neural learning algorithm is used to collect data from other sensors in the joint servo module that can measure or read torque. This relatively accurate rotational difference-torque relationship is further learned and corrected, ultimately resulting in a reliable torque measurement value.

[0057] The first rotation measuring device 1 and the second rotation measuring device 2 of this embodiment are used to measure the rotation angles of the input end and the output end of the harmonic reducer 3 , and can adopt rotation angle measuring devices commonly used in servo modules.

[0058] Due to the flexible transmission of the harmonic reducer 3, during the joint torque calculation process of this embodiment, the calculation data basis of the calculation module is not limited to the hardware-related parameters of the harmonic reducer 3, but can also be obtained based on the overall hardware-related parameters of the joint servo module installed on the harmonic reducer 3.

[0059] like Figure 1 As shown, the first rotation measuring device 1 is installed at the input end of the harmonic reducer 3 , and the second rotation measuring device 2 is installed at the output end of the harmonic reducer 3 .

[0060] This embodiment examines the relationship between the difference between the output angle of the second rotation measuring device 2 and the output angle of the first rotation measuring device 1 and the magnitude of the torque applied to the module, as shown in FIG. Figure 2 The relationship shown.

[0061] Figure 2 In, θ T It represents the difference between the two rotation angle measuring devices, and Te represents the output torque of the harmonic reducer 3.

[0062] In this embodiment, the measured torque is calculated based on the stiffness coefficient of the joint servo module / harmonic reducer 3 .

[0063] In this embodiment, generally, there is a certain correspondence between the rotation difference and the torque of the harmonic reducer 3. In this embodiment, the corresponding measured torque can be calculated based on data such as the stiffness coefficient of the harmonic reducer 3.

[0064] The method for calculating the torque based on the stiffness coefficient in this embodiment is implemented by the following formula:

[0065]

[0066] Where:

[0067] R: Gear reduction ratio of joint servo module / harmonic reducer 3;

[0068] T: measured torque;

[0069] θ in : Input angle of joint servo module / harmonic reducer 3;

[0070] θ out : Output angle of joint servo module / harmonic reducer 3;

[0071] Ks: Stiffness coefficient of joint servo module / harmonic reducer 3.

[0072] like Figure 3 As shown, this embodiment is based on the stiffness coefficient of the joint servo module or the harmonic reducer 3, and calculates the torque T according to the rotation difference (for example, when the first rotation measuring device 1 and the second rotation measuring device 2 use rotation angle measuring devices, the rotation difference is the angle difference). e1 , and then input both into the neural learning algorithm for learning operation, and finally get the deterministic θ T With T' e1 The relationship curve between .

[0073] Here, θ T is the angle difference of the dual-rotation angle measuring device; T' e1 is the calculated torque value.

[0074] In this embodiment, the method for calculating the torque based on the torque coefficient of the motor is implemented by the following formula:

[0075] T e =i qcmd *k t

[0076] Where:

[0077] Te: electromagnetic torque of the motor;

[0078] i qcmd : Current command on the motor q axis;

[0079] Kt: Torque coefficient of the motor.

[0080] If the stiffness coefficient of the joint servo module or the harmonic reducer 3 is unknown, theoretically, it is impossible to obtain accurate measured torque based on only two rotation differences. In this case, the measured torque can be calculated based on the torque coefficient of the motor (which can be obtained based on the current instruction), and the measured torque and the rotation angle difference at the corresponding moment are matched. Then, the neuron learning algorithm learns the data to obtain the corresponding relationship between the angle difference and the measured torque. The calculation process of this embodiment is as follows: Figure 4 shown.

[0081] From the above description, it can be seen that the present invention obtains the calculated torque by the following ways: based on the angle difference between two rotation angle measuring devices, based on the current command, and based on the torque observer. In this embodiment, Figure 5 As shown, in this embodiment, the measured torques obtained by the three methods can be substituted into the neuron learning algorithm of the data fusion unit to perform multi-sensor fusion calculations, so that the resulting mathematical relationship is more accurate, and the torque of the reducer is ultimately more accurate.

[0082] The joint torque of this embodiment can be used as the torque value of the actual servo module unit, and can be provided to the motor control system for reference, or participate in the control calculation of the actual motor control system.

[0083] In one embodiment, further, Figure 6 As shown, in this embodiment, the measured torques obtained from the three measurement methods are all collected together (the relevant data must be normalized first) and iteratively calculated, thereby ultimately obtaining a more accurate joint torque. The advantage of this embodiment is that it saves the computing power resources of the algorithm core and only uses the neuron learning algorithm to extract accurate torque from multiple data sources according to certain rules (such as the minimum mean square error rule).

[0084] In one embodiment of the present invention, Figure 7 As shown, the computing module includes an algorithm chip, a cache unit, and a data storage unit;

[0085] The algorithm chip is embedded with the logic of the neuron learning algorithm;

[0086] The cache unit exchanges data with the algorithm chip to cooperate with the neuron computing algorithm chip to execute the neuron learning algorithm.

[0087] In this embodiment, the computing module further includes a data storage unit; the data storage unit interacts with the algorithm chip to store operating data of the algorithm chip.

[0088] In this embodiment, the computation module communicates with the motor's control chip and torque observer to collect the q-axis current command and torque data from the torque observer. The neural learning algorithm uses an artificial neural network, but this embodiment does not specifically limit the logic of the artificial neural network.

[0089] In this embodiment, the algorithm chip is also used to feed back precise torque to the control chip; the control chip controls the motor with reference to the precise torque compensation.

[0090] In one embodiment, the input end rotation angle measuring device 1 and the output end rotation angle measuring device 2 are both rotation angle measuring devices; the rotation difference value is the angle difference calculated by substituting the angle difference between the input end rotation angle measuring device 1 and the output end rotation angle measuring device 2 into the gear reduction ratio.

[0091] In this embodiment, the algorithm chip is an FPGA or a DSP, the data storage unit is a FLASH DISK, and the control chip is a DSP.

[0092] like Figure 7 As shown, in this embodiment, two control cores can be set in the motor driver of the joint servo module. The first DSP serves as the control chip for executing the motor servo control algorithm, and the output signals of the two rotation angle measuring devices are input to the first DSP for control; the FPGA / second DSP serves as the core for executing the virtual torque sensor algorithm, and the angle difference between the two rotation angle measuring devices is input to the algorithm core; in addition, the current instruction and motor speed from the first DSP core are also input to the algorithm core; the algorithm core outputs the calculated measured torque to the first DSP core. In order to store historical operating data for the needs of the neuron learning algorithm, the FPGA / second DSP is configured with a large-capacity RAM and FLASH DISK. The parameters of the relevant servo modules, the stiffness coefficient of the harmonic reducer 3, the motor torque coefficient, etc. are set through the parameter setting module.

[0093] like Figure 7 As shown in the figure, a neuron-based learning algorithm is run in the FPGA to fuse the measured torque from the difference between the rotation angle meter, the measured torque from the current command, and the measured torque from the torque observer, and finally calculate the precise joint torque, realizing the virtual torque sensor function.

[0094] The FPGA and the second DSP can be set up simultaneously, and the second DSP runs the relevant parameter setting calculations and the relevant independently processed calculation algorithms, thereby assisting the neuron learning algorithm running in the FPGA to run more efficiently. For example, a torque observer is built in the second DSP to calculate the load torque; the electromagnetic torque is also calculated through the current command. The torque observer block diagram is as follows Figure 8 shown.

[0095] In the hardware framework built in this embodiment, multiple data sources are used to calculate joint torque, and the multiple data sources are processed by multi-sensor data fusion algorithm. Figure 9 Shown is the data processing architecture.

[0096] according to Figure 9As shown, the torque data calculated based on the angle difference data of the dual-rotation angle meter must first be input into the neuron learning algorithm. Through the iterative learning calculation of the unit, a relatively deterministic expression (mathematical relationship) of the relationship between the angle difference of the rotation angle meter and the torque is obtained; in addition, the measured torque based on the current instruction is also input into the neuron learning algorithm. Its purpose is to serve as a reference standard for the torque calculation value based on the dual-rotation angle meter. Finally, data fusion is performed based on the data fusion calculation unit. In the learning process of the neuron learning algorithm, those measured torques with amplitudes that are too large or too small will be eliminated, thereby purifying the learning data of the neuron learning algorithm and making its results closer to the true value.

[0097] The data fusion calculation unit of this embodiment is used to calculate the T e1 and T e2 ', and T from the torque observer e3 The data is fused and calculated to obtain a measured torque that is closer to the true value, making the torque calculation result more reliable and preventing the failure of the final joint torque calculation result due to the absence of a certain channel source or the abnormal instantaneous value of a certain channel source data.

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

Claims

1. The virtual torque sensor of the joint servo module based on multi-sensor fusion technology is characterized by: Used for torque measurement of joint servo module, the joint servo module realizes motor deceleration based on the harmonic reducer of flexible transmission; the virtual torque sensor includes: A first rotation measuring device is installed at the input end of the harmonic reducer and is used to measure the motor rotor angle data at the input end of the harmonic reducer; a second rotation measuring device, installed at the output end of the harmonic reducer, for measuring the rotation angle data of the output end of the harmonic reducer; a calculation module having a built-in neuron learning algorithm, communicating with the first rotation measuring device and the second rotation measuring device, and configured to collect a rotation difference between the first rotation measuring device and the second rotation measuring device; in: The calculation module includes a first control core and a second control core, wherein the first control core executes a motor servo control algorithm and receives output signals of two rotation angle meters, and the second control core executes a virtual torque sensor algorithm, receives the rotation difference, current command, and motor speed data, and outputs a calculated measured torque; The calculation module feeds back the measured torque to the first control core; The second control core is configured with a large-capacity RAM for storing historical operation data; The measured torque is calculated based on the rotation difference, based on the torque observer torque data of the joint servo module, and based on the current data of the motor; The neuron learning algorithm is configured to collect each of the rotation difference values, and then perform learning calculation based on the multi-sensor fusion technology by combining the rotation difference values ​​with the measured torque to obtain the precise torque of the harmonic reducer; The neuron learning algorithm is executed in the servo drive; The neuron learning algorithm configuration includes: Dynamically learning the measured torque calculated from the rotation difference and the stiffness coefficient of the harmonic reducer, and establishing a relationship expression model between the angle difference of the rotation angle measuring device and the torque; The relationship expression model is combined with the measured torque based on the current instruction for multi-sensor fusion, and the measured torque with an amplitude that is too large or too small is eliminated in real time to generate accurate torque.

2. The joint servo module virtual torque sensor based on multi-sensor fusion technology according to claim 1 is characterized in that: The method for obtaining the measured torque based on the rotation difference is achieved by the following formula: Where: : Gear reduction ratio of joint servo module / harmonic reducer; : Calculate torque; : Input angle of joint servo module / harmonic reducer; : Output angle of joint servo module / harmonic reducer; : Stiffness coefficient of joint servo module / harmonic reducer.

3. The joint servo module virtual torque sensor based on multi-sensor fusion technology according to claim 1 is characterized in that: The method for obtaining the measured torque based on the torque coefficient of the motor is implemented by the following formula: Where: : the electromagnetic torque of the motor; : Current command on the motor q axis; : Torque coefficient of the motor.

4. The joint servo module virtual torque sensor based on multi-sensor fusion technology according to claim 1, characterized in that: The computing module includes an algorithm chip, a cache unit and a data storage unit; The algorithm chip is embedded with the logic of the neuron learning algorithm; The cache unit interacts with the algorithm chip data to cooperate with the neuron computing algorithm chip to execute the neuron learning algorithm.

5. The joint servo module virtual torque sensor based on multi-sensor fusion technology according to claim 4 is characterized in that: The calculation module also includes a data storage unit; the data storage unit interacts with the algorithm chip data and is used to store the operation data of the algorithm chip.

6. The joint servo module virtual torque sensor based on multi-sensor fusion technology according to claim 5 is characterized in that: The calculation module communicates with the control chip of the motor and the torque observer, and is used to collect the current command on the q-axis of the motor and the torque data of the torque observer.

7. The joint servo module virtual torque sensor based on multi-sensor fusion technology according to claim 6 is characterized in that: The algorithm chip is further configured to feed back the precise torque to the control chip; the control chip controls the motor with reference to the precise torque compensation.

8. The joint servo module virtual torque sensor based on multi-sensor fusion technology according to claim 1 is characterized in that: The first rotation measuring device and the second rotation measuring device are both rotation angle measuring devices; The rotation difference value is an angle difference calculated by substituting the angle difference measured by the first rotation measuring device and the second rotation measuring device into the gear reduction ratio of the harmonic reducer.

Images