A method for determining the initial position of a rotor using a motor of a collaborative robot

Abstract

The present invention proposes a method for determining the initial position of the rotor of a collaborative robot motor, including: step S1, determining the angle recognition resolution of the collaborative robot rotor, and sequentially injecting high-frequency pulse voltages with different electrical angles according to a preset step angle; performing demodulation according to a design framework, and recording the electrical angle injected each time, the square of the rotor composite current amplitude, and the rotor position deviation caused by the backlash; step S2, searching for the minimum value in the stored square value of the rotor composite current amplitude, determining the angle of the corresponding point of the rotor position at this time, and judging the rotor polarity; step S3, adding the electrical angle of the corresponding point to the error caused by the brake backlash, and obtaining the final electrical angle of the initial position of the rotor. The present invention statically determines the rotor position when the robot does not move as a whole, thereby improving the accuracy of recognition.

Description

Technical Field

[0001] The present invention relates to the technical field of collaborative robots, and in particular to a method for determining an initial position of a rotor of a collaborative robot motor. Background Art

[0002] In recent years, with the gradual improvement of collaborative robot technology, the application areas of collaborative robots have been continuously developed from the automotive, electronic and electrical, machinery, and commercial industries to other application areas, becoming an indispensable part of many robot application occasions. Collaborative robots generally use frameless torque motors, which are surface-mounted permanent magnet synchronous motors. Since the collaborative robot body uses a hollow shaft for routing, the various parts of the joints are assembled separately. The zero position of the encoder cannot be aligned with the electrical angle zero point, and the initial position of the rotor needs to be determined before the motor starts.

[0003] The initial position of the rotor on a collaborative robot is generally determined by adding a DC voltage to the stator winding to move the motor rotor to the electrical angle zero position, thereby determining the initial position of the rotor. This method can be used to determine the initial position when the joint is not assembled to the whole machine. If the encoder of the robot is replaced or the motor needs to determine the initial position again, the robot cannot move in a limited working space, and this method is no longer applicable. Although some control methods can solve these problems when the rotor is stationary, it is still necessary to compensate for the errors caused by the signal processing path and the characteristics of the components themselves to further improve the recognition accuracy. Summary of the invention

[0004] The object of the present invention is to solve at least one of the technical drawbacks.

[0005] To this end, the purpose of the present invention is to provide a method for determining the initial position of a rotor of a collaborative robot motor.

[0006] In order to achieve the above object, an embodiment of the present invention provides a method for determining an initial position of a rotor of a collaborative robot motor, comprising the following steps:

[0007] Step S1, determine the angle recognition resolution of the collaborative robot rotor, and inject high-frequency pulse voltages with different electrical angles in sequence according to the preset step angle; demodulate according to the design framework, and record the electrical angle injected each time, the square of the rotor synthetic current amplitude, and the and the rotor position deviation θ caused by backlash comp ;

[0008] Step S2, storing the square value of the rotor composite current amplitude Find the minimum value in the equation to determine the angle of the corresponding point of the rotor position at this time Determine the rotor polarity;

[0009] Step S3, add the electrical angle of the corresponding point to the error θ caused by the brake backlash comp , obtain the final rotor initial position electrical angle θ.

[0010] Further, in step S1, the electrical angle setting value of the virtual coordinate system is incremented from 0° to 360° according to the step angle, and each step injects a high-frequency pulse voltage in sequence, injects a total of 360 voltages, performs voltage injection, and then obtains the synthetic current vector The square of .

[0011] Further, in step S1,

[0012] The injected frequency in the motor rotor dq rotating coordinate system is ω h A high frequency sinusoidal signal:

[0013] u dh =u amp *cos(ω h t);u qh =0

[0014] Ignore the motor impedance, and use the brake to lock the motor rotor, no back electromotive force is generated, then the motor voltage equation in steady state is:

[0015]

[0016] Among them, u dh Inject high-frequency sinusoidal voltage component into the d-axis, u qh is the q-axis voltage component with a value of 0, ω h is the frequency of the injected sinusoidal voltage, L dh is the d-axis high frequency inductance, L qh is the q-axis high-frequency inductance, i dh is the d-axis current component, i qh is the q-axis current component, z dh is the d-axis high frequency impedance, z qh is the q-axis high-frequency impedance;

[0017] Establishing an estimated rotation coordinate system The electrical angle is The electrical angle with the real rotating coordinate system dq is θ, the angle between the two coordinate systems is Δθ, and the injection frequency of the d-axis under the estimated coordinates is also ω h The high-frequency sinusoidal signal, the motor voltage equation in steady state is:

[0018]

[0019] in, To estimate the coordinate system The high-frequency sinusoidal voltage component is injected into the shaft. is an estimated coordinate system with a value of 0 Shaft voltage component, z avg is the average value of the high frequency inductive reactance of the d-axis and q-axis, z avg =(z dh +z qh ) / 2,z diff is the difference between the high frequency inductive reactance of the d-axis and the q-axis, z diff =z dh -z qh , To estimate the coordinate system The shaft current component, To estimate the coordinate system Shaft current component;

[0020] calculate The square of

[0021]

[0022]

[0023] Add the two together to get the resultant current vector The square of

[0024]

[0025] make

[0026] but

[0027] Further, in step S1, when estimating the coordinate system The deviation from the real coordinate system dq is the smallest, and the value of 2Δθ is close to 0. Minimum, resultant current vector The square amplitude of is the smallest, at this time the electrical angle of the estimated coordinate system is the electrical angle of the real coordinate system.

[0028] Further, the obtained and It contains DC components, which are extracted using a peak filter. and The frequency is ω h signal to ensure the accuracy of the amplitude square calculation.

[0029] Furthermore, before high-frequency voltage injection, the feedback value of the rotor's built-in encoder is recorded as the initial value. During the voltage injection process, the difference between the current encoder position feedback and the initial value is calculated and stored in real time. After the rotor position is determined, the deviation value is compensated.

[0030] Further, in step S3, the final rotor initial position electrical angle θ is obtained as:

[0031]

[0032] Further, in step S3, θ is calculated comp , record the number of rotor position feedback pulses P1 before voltage injection, record the number of rotor position feedback pulses P2 after voltage injection, the number of output pulses per motor rotation is EXT, the number of motor pole pairs is poles, then

[0033] θ comp =(P2-P1) / EXT*poles*360°.

[0034] The method for determining the initial position of a rotor of a motor used in a collaborative robot according to an embodiment of the present invention has the following beneficial effects:

[0035] (1) The rotor position is statically determined without any displacement of the entire robot.

[0036] (2) Improve the recognition accuracy through peak filter.

[0037] (3) By compensating for the error caused by the brake backlash, the accuracy of identification is further improved.

[0038] Additional aspects and advantages of the present invention will be given in part in the following description and in part will be obvious from the following description, or will be learned through practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] The above and / or additional aspects and advantages of the present invention will become apparent and easily understood from the description of the embodiments in conjunction with the following drawings, in which:

[0040] Figure 1 A flowchart of a method for determining an initial position of a rotor for a motor of a collaborative robot according to an embodiment of the present invention;

[0041] Figure 2 A block diagram of high frequency voltage injection according to an embodiment of the present invention;

[0042] Figure 3 Schematic diagram of a synthetic current vector magnitude square acquisition module according to an embodiment of the present invention;

[0043] Figure 4 is a Bode diagram of a peak filter according to an embodiment of the present invention;

[0044] Figure 5 FIG. 4 is a schematic diagram of estimating a rotation coordinate system according to an embodiment of the present invention. DETAILED DESCRIPTION

[0045] Embodiments of the present invention are described in detail below, 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 invention, and should not be construed as limiting the present invention.

[0046] The present invention provides a method for determining the initial position of a rotor of a collaborative robot motor, which is used to determine the initial position of the rotor when the motor rotor is in a stationary state.

[0047] like Figure 1 As shown, the method for determining the initial position of the rotor of a collaborative robot motor according to an embodiment of the present invention includes the following steps:

[0048] Step S1, determine the angle recognition resolution of the collaborative robot rotor, and inject high-frequency pulse voltages with different electrical angles in sequence according to the preset step angle; demodulate according to the design framework, and record the electrical angle injected each time, the square of the rotor synthetic current amplitude, and the and the rotor position deviation θ caused by backlash comp .

[0049] Specifically, the electrical angle setting value of the virtual coordinate system is incremented from 0° to 360° according to a certain step angle. The step angle of the present invention is set to 1° and the voltage is injected 360 times. Figure 2 Perform voltage injection and then obtain the synthetic current vector through the synthetic current extraction module The square of . Each time, three values ​​need to be recorded, which are the electrical angle of each injection Square of rotor composite current amplitude Encoder offset Δpulse (rotor position deviation caused by backlash θ comp )like Figure 2 As shown, the injection frequency in the motor rotor dq rotating coordinate system is ω h A high frequency sinusoidal signal:

[0050] u dh =u amp *cos(ω h t)

[0051] u qh =0

[0052] Ignore the motor impedance, and use the brake to lock the motor rotor, no back electromotive force is generated, then the motor voltage equation in steady state is:

[0053]

[0054] Among them, u dh Inject high-frequency sinusoidal voltage component into the d-axis, u qhis the q-axis voltage component with a value of 0, ω h is the frequency of the injected sinusoidal voltage, L dh is the d-axis high frequency inductance, L qh is the q-axis high-frequency inductance, i dh is the d-axis current component, i qh is the q-axis current component, z dh is the d-axis high frequency impedance, z qh is the q-axis high-frequency impedance.

[0055] Since the true electrical angle of the rotor is unknown, an estimated rotating coordinate system is established The electrical angle is The electrical angle with the real rotating coordinate system dq is θ, and the angle between the two coordinate systems is Δθ. Figure 5 FIG. 4 is a schematic diagram of estimating a rotation coordinate system according to an embodiment of the present invention.

[0056] The injection frequency of the d-axis under the estimated coordinates is also ω h The high-frequency sinusoidal signal, the motor voltage equation in steady state is:

[0057]

[0058] in To estimate the coordinate system The high-frequency sinusoidal voltage component is injected into the shaft. is an estimated coordinate system with a value of 0 Shaft voltage component, z avg is the average value of the high frequency inductive reactance of the d-axis and q-axis, z diff is the difference between the high frequency inductive reactance of the d-axis and the q-axis, To estimate the coordinate system The shaft current component, To estimate the coordinate system Shaft current component.

[0059] z avg =(z dh +z qh ) / 2

[0060] z diff =z dh -z qh

[0061] From the above formula we can get:

[0062]

[0063]

[0064] calculate The square of

[0065]

[0066]

[0067] Add the two together to get the resultant current vector The square of

[0068]

[0069] make

[0070] but

[0071]

[0072] The resultant current vector The square of the low-pass filter is used for low-pass filtering. The output value of the low-pass filter is

[0073]

[0074] When the deviation between the estimated coordinate system and the true coordinate system is the smallest, the value of 2Δθ is close to 0. Minimum, resultant current vector The square amplitude is the smallest, and the electrical angle of the estimated coordinate system is the electrical angle of the real coordinate system. According to this principle. The electrical angle setting value of the virtual coordinate system is incremented from 0° to 360° according to a certain step angle. The step angle of the present invention is set to 1° and a total of 360 voltage injections are performed. The voltage is injected through the synthetic current vector amplitude square acquisition module, and then the synthetic current vector is obtained through the synthetic current extraction module. The square of . Figure 3 Schematic diagram of a synthetic current vector magnitude square acquisition module according to an embodiment of the present invention.

[0075] In an embodiment of the present invention, the obtained and It contains a DC component, in addition to the injected voltage frequency ω h In order to ensure the accuracy of the square calculation of the amplitude, it is necessary to accurately extract the frequency components other than and The frequency is ω h Signal. Figure 4 The present invention uses a peak filter to extract the Bode diagram of the peak filter according to the embodiment of the present invention. and The frequency is ω h Signal. The peak filter is a special bandpass filter with the characteristics of no amplitude attenuation and no phase lag at a specific frequency. The peak filter transfer function is:

[0076]

[0077] Step S2, storing the square value of the rotor composite current amplitude Find the minimum value in the equation to determine the angle of the corresponding point of the rotor position at this time Determine the rotor polarity.

[0078] Specifically, electrical angle From 0° to 360°, the recorded It can be found that there are two minimum values ​​in one identification cycle, refer to the following formula

[0079]

[0080] It can be seen that The characteristic of Δθ being twice the frequency is that there are two minimum values, and we need to determine the corresponding Which is the correct electrical angle. The two electrical angles of the minimum value are: Set the electrical angle to Keep the brake closed, and inject a DC voltage signal of a certain amplitude at the current electrical angle. The voltage is injected in the positive direction for 500ms and in the negative direction for 500ms. The d-axis current i during the positive voltage injection period is collected respectively. d1 and the q-axis current i during negative voltage injection d2 , if the forward voltage i d1 The absolute value is greater than i d2 The absolute value of is the rotor electrical angle, otherwise The electrical angle.

[0081] Step S3, add the electrical angle of the corresponding point to the error θ caused by the brake backlash comp , obtain the final rotor initial position electrical angle θ.

[0082] Determine the electrical angle. Take this as an example, and use the following formula to compensate for the backlash to obtain the final rotor position electrical angle θ:

[0083]

[0084] Calculate θ comp , using the following method: record the number of rotor position feedback pulses P1 before voltage injection, record the number of rotor position feedback pulses P2 after voltage injection, the number of output pulses per motor rotation is EXT, the number of motor pole pairs is poles, then

[0085] θ comp =(P2-P1) / EXT*poles*360°.

[0086] In summary, the method for determining the initial position of the rotor of the motor of the collaborative robot of the embodiment of the present invention, when injecting high-frequency voltage to calculate the synthetic current amplitude, locks the motor rotor through the brake. Due to the gap in the brake, the rotor will rotate slightly during the injection of the high-frequency voltage, which will cause errors in the rotor position identification and needs to be compensated. The robot joint generally uses a reducer with a larger reduction ratio of about 100, so the slight rotation caused by the backlash of the brake will not cause obvious movement at the end after passing through the reducer. Before the high-frequency voltage is injected, the feedback value of the rotor's own encoder is recorded as the initial value. During the voltage injection process, the difference between the current encoder position feedback and the initial value is calculated in real time and stored. After the rotor position is determined, the deviation value is compensated.

[0087] Taking 131072 pulses in one circle as an example, the initial position encoder feedback is 4500. When the virtual rotating coordinate system is aligned with the real rotating coordinate system, the synthetic current vector amplitude is the smallest. The angle is 35°, the encoder feedback is 4540, the deviation from the initial value is 40 pulses, and the motor has 8 pairs of poles. The calculation formula for the electrical angle corresponding to the offset Δpulse40 pulses is:

[0088] The final electrical angle is:

[0089] The method for determining the initial position of a rotor of a motor used in a collaborative robot according to an embodiment of the present invention has the following beneficial effects:

[0090] (1) The rotor position is statically determined without any displacement of the entire robot.

[0091] (2) Improve the recognition accuracy through peak filter.

[0092] (3) By compensating for the error caused by the brake backlash, the accuracy of identification is further improved.

[0093] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "examples", "specific examples", or "some examples" means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representation of the above terms does not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner.

[0094] Although the embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and cannot be understood as limiting the present invention. Those skilled in the art may change, modify, replace and modify the above embodiments within the scope of the present invention without departing from the principles and purpose of the present invention. The scope of the present invention is defined by the appended claims and their equivalents.

Claims

1. A method for determining the initial position of a rotor of a collaborative robot motor, characterized in that: The steps include: Step S1, determine the angle recognition resolution of the collaborative robot rotor, and inject high-frequency pulse voltages with different electrical angles in sequence according to the preset step angle; demodulate according to the design framework, and record the electrical angle injected each time, the square of the rotor synthetic current amplitude, and the and the rotor position deviation θ caused by backlash comp ; Step S2, storing the square value of the rotor composite current amplitude Find the minimum value in the equation to determine the angle of the corresponding point of the rotor position at this time Determine the rotor polarity; Step S3, add the electrical angle of the corresponding point to the error θ caused by the brake backlash comp , obtain the final rotor initial position electrical angle θ, where the final rotor initial position electrical angle θ is obtained as: Calculate θ comp , record the number of rotor position feedback pulses P1 before voltage injection, record the number of rotor position feedback pulses P2 after voltage injection, the number of output pulses per motor rotation is EXT, the number of motor pole pairs is poles, then i comp = (P2-P1) / EXT*poles*360°.

2. The method for determining the initial position of a rotor of a motor for a collaborative robot according to claim 1, characterized in that: In step S1, the electrical angle setting value of the virtual coordinate system is incremented from 0° to 360° according to the step angle, and each step injects a high-frequency pulse voltage in sequence, injects a total of 360 voltages, performs voltage injection, and then obtains the synthetic current vector The square of .

3. The method for determining the initial position of a rotor of a motor for a collaborative robot according to claim 1 or 2, characterized in that: In step S1, The injected frequency in the motor rotor dq rotating coordinate system is ω h A high frequency sinusoidal signal: in dh =in amp *cos(ω h here qh =0 Ignore the motor impedance, and use the brake to lock the motor rotor, no back electromotive force is generated, then the motor voltage equation in steady state is: Among them, u dh Inject high-frequency sinusoidal voltage component into the d-axis, u qh is the q-axis voltage component with a value of 0, ω h is the frequency of the injected sinusoidal voltage, L dh is the d-axis high frequency inductance, L qh is the q-axis high-frequency inductance, i dh is the d-axis current component, i qh is the q-axis current component, z dh is the d-axis high frequency impedance, z qh is the q-axis high-frequency impedance; Establishing an estimated rotation coordinate system The electrical angle is The electrical angle with the real rotating coordinate system dq is θ, the angle between the two coordinate systems is Δθ, and the injection frequency of the d-axis under the estimated coordinates is also ω h The high-frequency sinusoidal signal, the motor voltage equation in steady state is: in, To estimate the coordinate system The high-frequency sinusoidal voltage component is injected into the shaft. is an estimated coordinate system with a value of 0 Shaft voltage component, z avg is the average value of the high frequency inductive reactance of the d-axis and q-axis, z avg =(z dh +z qh ) / 2,z diff is the difference between the high frequency inductive reactance of the d-axis and the q-axis, z diff =z dh -z qh , To estimate the coordinate system The shaft current component, To estimate the coordinate system Shaft current component; calculate The square of Add the two together to get the resultant current vector The square of make but 4. The method for determining the initial position of a rotor of a motor for a collaborative robot as claimed in claim 3, characterized in that: In step S1, when estimating the coordinate system The deviation from the real coordinate system dq is the smallest, and the value of 2Δθ is close to 0. Minimum, resultant current vector The square amplitude of is the smallest, at this time the electrical angle of the estimated coordinate system is the electrical angle of the real coordinate system.

5. The method for determining the initial position of a rotor of a motor for a collaborative robot according to claim 4, characterized in that: Obtained and It contains DC components, which are extracted using a peak filter. and The frequency is ω h signal to ensure the accuracy of the amplitude square calculation.

6. The method for determining the initial position of a rotor of a motor for a collaborative robot according to claim 1, characterized in that: Before high-frequency voltage injection, the feedback value of the rotor's built-in encoder is recorded as the initial value. During the voltage injection process, the difference between the current encoder position feedback and the initial value is calculated and stored in real time. After the rotor position is determined, the deviation value is compensated.

Images