Improved robot joint motor initial position detection method

Abstract

The invention belongs to the technical field of robots, and particularly relates to an improved robot joint motor initial position detection method. The method comprises the following improvement steps: adding a magnetic pole identification program to eliminate 180-degree detection errors; the magnitude and time of injection voltage in each process after magnetic pole identification are increased, the influence of sampling errors is reduced, and the detection precision is improved; and a curve fitting algorithm is added to fit a current curve near an actual rotor of the motor, so that an initial position is found out. Therefore, the joint motor system adopting the incremental encoder can obtain the initial position of the motor more accurately. According to the invention, the initial position of the motor can be accurately obtained without rotating the joint motor system adopting the incremental encoder to obtain the Z signal.

Description

Technical Field

[0001] This invention belongs to the field of robotics technology, specifically an improved method for detecting the initial position of robot joint motors. Background Technology

[0002] The application of robotics technology has penetrated into all aspects of our factory production and social life, from large-scale industrial manufacturing to delicate medical surgeries, from distant space exploration to everyday household services, and has become an integral part of human life. Its core value lies in replacing humans in repetitive, tedious, dangerous, and precise tasks, or in enhancing human capabilities.

[0003] The core of driving a robot's movement is the motor. The coordinated operation of the motors at each joint drives the robot's joints, achieving the desired motion posture. Therefore, the motor system is the core component of the entire robot control system. The key to joint motor rotation is accurate startup, which requires obtaining the initial position of the joint motor. Starting the motor blindly without obtaining the initial position can cause overcurrent, reverse rotation, or jamming, potentially leading to very serious consequences in practical production applications. The more accurate the initial position, the greater the starting torque and the higher the operating efficiency of the motor. For joint motor systems using absolute encoders, the initial position information can be directly obtained by reading the encoder data. However, for joint motor systems using incremental encoders, the initial position information cannot be read from the encoder during the initial startup after a power outage; rotation to obtain the Z-signal is required to determine the initial position. However, some operating conditions require the motor to remain stationary during startup, making initial position detection technology essential. When using the traditional voltage pulse injection method to detect the initial position of a motor rotor, the difference between the D-axis feedback currents is not significant when the voltage vector position is close to the actual angle. Due to current sampling errors, misjudgments can occur, making it difficult to achieve extremely high accuracy. Furthermore, when the magnetizing or weakening effect of the current is weak, the distinction between the D-axis currents fed back by the voltage injected at the rotor's N and S poles is insufficient, easily leading to a 180-degree detection error. Summary of the Invention

[0004] To address the aforementioned issues, this invention provides an improved method for detecting the initial position of a robot joint motor, enabling a joint motor system employing an incremental encoder to accurately obtain the initial position of the motor without needing to rotate to acquire the Z signal.

[0005] The technical solution of this invention is described below in conjunction with the accompanying drawings:

[0006] This invention provides an improved method for detecting the initial position of a robot joint motor, comprising the following steps:

[0007] Step 1: Select the magnitude of the injected voltage vector based on the motor parameters. and injection time ;

[0008] Step 2: In the first round, inject 12 voltage vectors V1-V12, with the angles of the 12 voltage vectors differing from each other. ,at first The angle is 30°, and the peak current of the feedback is compared. Select the largest in the first round The voltage vector angle corresponding to the value is used as the output angle of this round. Then, a magnetic pole identification program is added to eliminate the 180° detection error;

[0009] Step 3, Second Round of Selection , , The voltage vectors corresponding to the three angles are the second-round injected voltage vectors, and then the maximum feedback current is selected. The corresponding voltage vector angle is used as the output angle of this round. In the second round of voltage vector injection, the magnitude and duration of the injection voltage are increased.

[0010] Step 4, Third Round of Selection , , The voltage vectors corresponding to the three angles are the third round of injected voltage vectors, and then the maximum feedback current is selected. The corresponding voltage vector angle is used as the output angle of this round. In the third round of voltage vector injection, the magnitude and duration of the injection voltage are increased.

[0011] Step 5, Fourth Round of Selection , , The voltage vectors corresponding to the three angles are the fourth round of injected voltage vectors, and then the maximum feedback current is selected. The corresponding voltage vector angle is used as the output angle of this round. In the fourth round of voltage vector injection, the magnitude and duration of the injection voltage are increased.

[0012] Step Six: Fifth Round of Selection , , The voltage vectors corresponding to the three angles are the fifth round of injected voltage vectors, and then the maximum feedback current is selected. The corresponding voltage vector angle is used as the output angle of this round. In the fifth round of voltage vector injection, the magnitude and duration of the injection voltage are increased.

[0013] Step 7: Add a curve fitting algorithm to fit the current curve based on the obtained information. The angle corresponding to the maximum value of the curve is taken as the final detection result.

[0014] Furthermore, the specific method for step one is as follows:

[0015] The injected voltage vector The size should be such that the motor rotor does not rotate or only vibrates during the test; the injection time The selection of the value should ensure that the response current returns to zero within this time period.

[0016] Furthermore, in step two, the injected voltage vector The injection sequence is as follows: after injecting one voltage vector, inject another voltage vector in the opposite direction; the magnetic pole identification procedure specifically involves comparing the injection angles. , When the two voltage vectors Value size, determination The larger the value, the correct detection angle is.

[0017] Furthermore, the principle for increasing the magnitude and time of the injection voltage during the second, third, fourth, and fifth rounds of injection voltage vector injection should be: to improve the differentiation between feedback currents without causing the motor rotor to be directly dragged to the current injection voltage vector angle.

[0018] Furthermore, the specific method for step seven is as follows:

[0019] Utilizing the acquired current Based on the angle information, curve fitting is performed to find the initial position; according to The expression, Within the range of +10° to -10°, the curve with the current forms a quadratic curve. This quadratic curve is fitted using the obtained information to find the initial position. After completing step six, the angle information corresponding to the three voltage vectors V24, V25, and V26 is used... , , And the corresponding feedback received , , Information is used to perform Lagrange curve fitting; by fitting, the relationship with the injection voltage angle is obtained. After the curve, the angle corresponding to the maximum value is found as the final initial angle detection result.

[0020] Furthermore, the expression for the Lagrange curve is as follows:

[0021] (8)

[0022] (9)

[0023] In the formula, This is the expression for the fitted curve; , For independent variable data; For dependent variable data; are basis functions;

[0024] Angle information , , and the corresponding , , Substituting the information into formulas (8) and (9) and solving the quadratic equation in one variable, i.e. formula (10), we can obtain the quadratic polynomials (11), (12), (13), and (14) related to the angle and the current value.

[0025] (10)

[0026] (11)

[0027] (12)

[0028] (13)

[0029] (14)

[0030] According to the formula for the extrema of a quadratic function, we get The angle value corresponding to the maximum value is:

[0031] (15)

[0032] Finally, this value is used as the final test result.

[0033] The beneficial effects of this invention are as follows:

[0034] 1) This invention, by introducing a magnetic pole identification program, can eliminate a 180° detection error.

[0035] 2) This invention improves detection accuracy by increasing the magnitude and duration of the injected voltage in each process after magnetic pole identification, thereby reducing the impact of sampling errors;

[0036] 3) This invention incorporates a curve fitting algorithm to fit the current curve, thereby finding the initial position. This allows the joint motor system using an incremental encoder to obtain the initial position of the motor more accurately without rotating to obtain the Z signal. Attached Figure Description

[0037] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings used in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of the present invention and should not be regarded as a limitation on the scope. For those skilled in the art, other related drawings can be obtained based on these drawings without creative effort.

[0038] Figure 1 A schematic diagram showing the curves of injection voltage and its response current over time.

[0039] Figure 2 Here is a flowchart of the voltage pulse injection method;

[0040] Figure 3 This is a voltage injection sequence diagram;

[0041] Figure 4 Flowchart for the improved voltage pulse injection method;

[0042] Figure 5 A comparison chart of detection results between the improved method and the traditional method. Detailed Implementation

[0043] The present invention will now be described in further detail with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and not intended to limit it. Furthermore, it should be noted that, for ease of description, the accompanying drawings show only the parts relevant to the present invention, and not all of the structures.

[0044] Example 1

[0045] The main principle of the voltage pulse injection method is as follows: By injecting a series of voltage vectors with fixed angle differences into the motor, the D-axis current generated by the corresponding voltage vectors is collected, and the angle of the voltage vector corresponding to the largest D-axis current value is selected as the final identification angle for each round. Through continuous iteration, extremely high identification accuracy can theoretically be obtained.

[0046] (1)

[0047] (2)

[0048] In the formula, , For dq axis inductance; Stator resistance; The angle between the injected voltage position and the actual initial position of the motor; t is the voltage injection time; , To provide feedback on the magnitude of the dq-axis current; This refers to the magnitude of the injected voltage.

[0049] For built-in permanent magnet synchronous motors, due to their structural characteristics... Surface-mounted permanent magnet synchronous motors can also achieve magnetic saturation through the magnetic saturation effect. Thus making <0. According to equations (1) and (2), it can be derived that when the injected voltage vector is When the value is 0, the maximum value can be obtained. Therefore, based on this principle, the initial angle information can be detected by injecting a series of voltage vectors and comparing their feedback values.

[0050] Based on the above principles, the initial rotor position can be detected by injecting a series of voltage vectors with the same angle difference into the motor stator and then judging the magnitude of the feedback current. To ensure that the injected voltage vectors do not interfere with each other, the injection voltage is stopped for a period of time after a certain period of injection. After the voltage is reduced to zero, another voltage vector is injected. The curves showing the changes of the injected voltage and its response current over time are as follows: Figure 1 As shown.

[0051] (3)

[0052] (4)

[0053] In the formula, For voltage injection time; This refers to the voltage injection stop time.

[0054] By converting three-phase current , , The sampled values ​​are obtained by varying Clark and Park as shown in formula (5):

[0055] (5)

[0056] In the formula, , , These are the sampled three-phase current values; The angle of the injected voltage.

[0057] See Figures 1-4 This embodiment provides an improved method for detecting the initial position of a robot joint motor, including the following steps:

[0058] Step 1: Select the magnitude of the injected voltage vector based on the motor parameters. and injection time The specific method is as follows:

[0059] The injected voltage vector The size should be such that the motor rotor does not rotate or only vibrates during the test; the injection time The selection of the value should ensure that the response current returns to zero within this time period.

[0060] Step 2: In the first round, inject 12 voltage vectors V1-V12, with the angles of the 12 voltage vectors differing from each other. ,at first It is 30°.

[0061] By comparing the feedback peak current Select the largest in the first round The voltage vector angle corresponding to the value is used as the output angle of this round. .

[0062] The voltage vector injection sequence is as follows: Figure 3 As shown: After injecting one voltage vector, inject another voltage vector in the opposite direction;

[0063] When the magnetizing and weakening effects of the motor stator magnetic field are present, the current generated by the voltage in the same direction as the motor's initial position cannot be much greater than the current generated by the voltage in the opposite direction to the motor's initial position. Due to the existence of certain detection errors, the detection result will have a detection error of 180 degrees.

[0064] In equation (1) Differentiation yields:

[0065] (6)

[0066] Because the voltage pulse injection method relies on feedback The initial position is determined by comparing the values. As can be seen from equation (6), with... As it gradually approaches 0, The derivative gradually approaches 0. Therefore, the derivatives of different voltage vectors... The difference between them gradually decreases. At this point, the presence of sampling error can lead to incorrect judgments, resulting in a decrease in detection accuracy.

[0067] Therefore, the results of this round of testing can be obtained after the first round. To verify whether the result is correctly located near the actual initial angle rather than in the opposite direction of the actual angle, an injection angle of... , The two voltage vectors, and for The correct detection angle is selected by comparing the numerical values; the angle corresponding to the larger current value is correct. Furthermore, increasing the injection voltage during magnetic pole identification enhances the magnetization and weakening effects of the stator magnetic field, thereby improving... , The two voltage vector feedback currents have a high degree of differentiation, thus correctly detecting the magnetic pole direction. Since the angle of the injected voltage is very close to the actual initial position of the motor, The voltage is relatively low, and according to formula (7), increasing the value of the injection voltage will not generate excessive torque that would cause the motor to rotate.

[0068] (7)

[0069] In the formula, This represents the number of pole pairs of the motor. For permanent magnet flux linkage in motors.

[0070] Step 3, Second Round of Selection , , The voltage vectors corresponding to the three angles are used as the second-round injection voltage vectors, and then the maximum feedback current is selected. The corresponding voltage vector angle is used as the output angle of this round. ;

[0071] Step 4, Third Round of Selection , , The voltage vectors corresponding to the three angles are used as the third round of injected voltage vectors, and then the maximum feedback current is selected. The corresponding voltage vector angle is used as the output angle of this round. ;

[0072] Step 5, Fourth Round of Selection , , The voltage vectors corresponding to the three angles are used as the fourth round of injected voltage vectors, and then the maximum feedback current is selected. The corresponding voltage vector angle is used as the output angle of this round. ;

[0073] Step Six: Fifth Round of Selection , , The voltage vectors corresponding to the three angles are used as the fifth round of injected voltage vectors, and then the maximum feedback current is selected. The corresponding voltage vector angle is used as the output angle of this round. ;

[0074] Specifically, the magnitude and duration of the injection voltage are increased in the second, third, and fourth rounds of the injection voltage vectoring process, and further increases are required in the fifth round. Larger injection voltages and durations can increase the flux linkage saturation, thereby reducing... The size can be increased to obtain a larger size. And the effect of increasing the injection voltage at different angles The differences between them. Therefore, in the second, third, fourth, and fifth injection processes after magnetic pole identification, the magnitude and duration of the injection voltage can be increased. The increased injection voltage should be sufficient to improve the differentiation between feedback currents without causing the motor rotor to be directly dragged to the current injection voltage vector angle. This reduces sampling errors and the impact of disturbances on the detection results, thereby improving detection accuracy.

[0075] Step 7: Add a curve fitting algorithm to obtain the curve of the motor rotor position versus the current. This will help determine the precise initial position.

[0076] Utilizing the acquired current Based on the angle information, curve fitting is performed to find the initial position; according to The expression, Within the range of +10° to -10°, its curve with the current is a quadratic curve. The obtained information is used to fit this quadratic curve to find the initial position. After completing step six, the angle information corresponding to the voltage vectors V24, V25, and V26 is used. , , And the corresponding feedback received , , Information is used to perform Lagrange curve fitting; by fitting, the relationship with the injection voltage angle is obtained. After the curve, the angle corresponding to the maximum value is found as the final initial angle detection result.

[0077] The expression for the Lagrange curve is as follows:

[0078] (8)

[0079] (9)

[0080] In the formula, This is the expression for the fitted curve; , Represents the independent variable data; Represents the dependent variable data. These are basis functions.

[0081] Angle information , , and the corresponding , , Substituting the information into formulas (8) and (9) and solving the quadratic equation in one variable, i.e. formula (10), we can obtain the quadratic polynomials (11), (12), (13), and (14) related to the angle and the current value.

[0082] (10)

[0083] (11)

[0084] (12)

[0085] (13)

[0086] (14)

[0087] According to the formula for the extrema of a quadratic function, we get The angle value corresponding to the maximum value is:

[0088] (15)

[0089] Finally, this value is used as the final test result.

[0090] Example 2

[0091] See Figure 5 , Figure 5 As shown in the figure, the present invention achieves higher detection accuracy and eliminates the 180-degree error when using the traditional method and the improved method of Embodiment 1 to detect the initial position of the robot joint motor.

[0092] Although embodiments of the invention have been shown and described, it will be understood by those skilled in the art that various changes, modifications, substitutions and alterations can be made to these embodiments without departing from the principles and spirit of the invention, the scope of which is defined by the appended claims and their equivalents.

Claims

1. An improved method for detecting the initial position of a robot joint motor, characterized in that, Includes the following steps: Step 1: Select the magnitude of the injected voltage vector based on the motor parameters. and injection time ; Step 2: In the first round, inject 12 voltage vectors V1-V12, with the angles of the 12 voltage vectors differing from each other. ,at first The angle is 30°, and the peak current of the feedback is compared. Select the largest in the first round The voltage vector angle corresponding to the value is used as the output angle of this round. Then, a magnetic pole identification program is added to eliminate the 180° detection error; Step 3: Second Round of Selection , , The voltage vectors corresponding to the three angles are the second-round injected voltage vectors, and then the maximum feedback current is selected. The corresponding voltage vector angle is used as the output angle of this round. In the second round of voltage vector injection, the magnitude and duration of the injection voltage are increased. Step 4, Third Round of Selection , , The voltage vectors corresponding to the three angles are the third round of injected voltage vectors, and then the maximum feedback current is selected. The corresponding voltage vector angle is used as the output angle of this round. In the third round of voltage vector injection, the magnitude and duration of the injection voltage are increased. Step 5, Fourth Round of Selection , , The voltage vectors corresponding to the three angles are the fourth round of injected voltage vectors, and then the maximum feedback current is selected. The corresponding voltage vector angle is used as the output angle of this round. In the fourth round of voltage vector injection, the magnitude and duration of the injection voltage are increased. Step Six: Fifth Round of Selection , , The voltage vectors corresponding to the three angles are the fifth round of injected voltage vectors, and then the maximum feedback current is selected. The corresponding voltage vector angle is used as the output angle of this round. In the fifth round of voltage vector injection, the magnitude and duration of the injection voltage are increased. Step 7: Add a curve fitting algorithm to fit the current curve based on the obtained information. The angle corresponding to the maximum value of the curve is taken as the final detection result.

2. The improved method for detecting the initial position of a robot joint motor according to claim 1, characterized in that, The specific method for step one is as follows: The injected voltage vector The size should be such that the motor rotor does not rotate or only vibrates during the test; the injection time The selection of the value should ensure that the response current returns to zero within this time period.

3. The improved method for detecting the initial position of a robot joint motor according to claim 1, characterized in that, In step two, the injected voltage vector The injection sequence is as follows: after injecting one voltage vector, inject another voltage vector in the opposite direction; The magnetic pole identification procedure specifically involves comparing the injection angle. , When two voltage vectors Value size, determination The larger the value, the correct detection angle is.

4. The improved method for detecting the initial position of a robot joint motor according to claim 1, characterized in that, The principle for increasing the magnitude and time of the injected voltage during the second, third, fourth, and fifth rounds of voltage vector injection should be to improve the differentiation between feedback currents without causing the motor rotor to be directly dragged to the current injected voltage vector angle.

5. The improved method for detecting the initial position of a robot joint motor according to claim 1, characterized in that, The specific method for step seven is as follows: Utilizing the acquired current Based on the angle information, curve fitting is performed to find the initial position; according to The expression, Within the range of +10° to -10°, the curve with the current forms a quadratic curve. This quadratic curve is fitted using the obtained information to find the initial position. After completing step six, the angle information corresponding to the three voltage vectors V24, V25, and V26 is used... , , And the corresponding feedback received , , Information is used to perform Lagrange curve fitting; by fitting, the relationship with the injection voltage angle is obtained. After the curve, the angle corresponding to the maximum value is found as the final initial angle detection result.

6. An improved method for detecting the initial position of a robot joint motor according to claim 5, characterized in that, The expression for the Lagrange curve is as follows: （8） （9） In the formula, This is the expression for the fitted curve; , For independent variable data; For dependent variable data; are basis functions; Angle information , , and the corresponding , , Substituting the information into formulas (8) and (9) and solving the quadratic equation in one variable, i.e. formula (10), we can obtain the quadratic polynomials (11), (12), (13), and (14) related to the angle and the current value. （10） （11） （12） （13） （14） According to the formula for the extrema of a quadratic function, we get The angle value corresponding to the maximum value is: （15） Finally, this value is used as the final test result.

Images