Single incremental position sensor robot joint motor initial position identification method

Abstract

The invention provides an initial position identification method of a single incremental position sensor robot joint motor. A hardware platform of the algorithm comprises a motor driver (1), a motor (2), a speed reducer (3), an incremental position sensor (4), a current injection algorithm module (6) operated by a main control chip in a motor controller (5), and an initial position identification algorithm module (7). Square wave current with a certain specific angle is injected into the motor through the motor driver, so that the motor generates micro motion with extremely small amplitude, the incremental position sensor generates a signal and outputs the signal to the initial position identification algorithm module, and the identification algorithm module outputs a changed current injection angle. And finally, converging to an initial position angle of the motor rotor, completing identification of an initial position, and realizing magnetic pole detection in the identification process. According to the invention, the problem that most robot joint motors without salient polarity cannot realize initial position detection in a static state is solved.

Description

Technical Field

[0001] The present invention relates to a method for identifying the initial position of a robot joint motor using only a single incremental position sensor, belonging to the technical field of motor drive. Background Art

[0002] Currently, in the high-precision control of robot joint motors, the initial position information of the rotor is indispensable. When a single incremental encoder is installed at the end of the robot joint to output position information, since the robot joint may rotate during power-off, the initial position of the motor rotor is random, and it is difficult to obtain this information. To obtain the initial position information for the smooth start of the motor and achieve high-precision control, the existing pre-positioning methods will cause the joint motor to rotate by a certain angle, which needs to be avoided in some scenarios with high-precision requirements; while using a sensorless control algorithm such as the high-frequency injection method to detect the initial position depends on the saliency of the joint motor itself, and most robot joint motors are surface-mounted permanent magnet synchronous motors without saliency. Therefore, there is currently a lack of an effective initial position detection method for robot joint motors with a single incremental position sensor. Summary of the Invention

[0003] Technical Problem: The present invention provides a method for identifying the initial position of the rotor for a robot joint motor with a single incremental position sensor to solve the problem of difficult initial position identification for such robot joints during startup. This method injects a square-wave current into the motor to make the motor output torque, thereby causing the motor rotor to move slightly and the incremental encoder to output a position signal. Using this position signal for feedback control provides high-precision initial position information of the rotor for the smooth start of the robot joint motor when the rotor is in a quasi-stationary state, thus solving the drawback that the current robot joint can only obtain the initial position of the motor by using a dual-encoder configuration or installing an absolute encoder on the motor side, reducing the structural complexity and installation cost of the robot joint. At the same time, problems such as rotor movement or dependence on motor saliency in the existing initial position detection methods are avoided.

[0004] Technical Solution: To achieve the identification of the initial position, the technical solution specifically adopted by the initial position identification method of a single incremental position sensor robot joint motor of the present invention is as follows:

[0005] The robot joint includes a motor driver, a robot joint motor, a reducer, an incremental position sensor, a motor controller, and a current injection algorithm module and a position identification algorithm module operating in its main control chip. Among them, the motor driver, the robot joint motor, the reducer, and the incremental position sensor are sequentially connected. The incremental position sensor outputs a position signal to the position identification algorithm module. The position identification algorithm module outputs the angle information of the injected square-wave current to the current injection algorithm module. The current injection algorithm module calculates the amplitude and period and outputs a PWM signal to the motor driver. The motor controller controls the current injection algorithm module and the position identification algorithm module respectively to realize the identification of the initial position angle of the robot joint motor and the pole detection.

[0006] In the current injection algorithm module, the square-wave current injection calculation outputs a square-wave current i ds to current control. The injection angles θ1 and θ2 are connected to current control through a switching switch. The output of the current control is connected to coordinate transformation. The output of the coordinate transformation is connected to SVPWM, and SVPWM outputs a PWM signal.

[0007] The square-wave current i ds has a waveform that is a square wave leading by a quarter cycle within one cycle, that is, a waveform of positive, negative, negative, positive. After injecting this square-wave current for one cycle, the rotor of the robot joint motor will rotate forward by the maximum displacement angle θ max mechanical angle in the first half cycle, and will rotate backward by the same angle and return to the original position in the second half cycle. At the same time, the rotational speed of the motor also drops to 0. The square-wave current i ds is specifically defined as follows:

[0008]

[0009] where I div is the amplitude of the square-wave current; T is the square-wave period of the current signal i ds ; t is the injection time of the square-wave current i ds .

[0010] The maximum displacement angle θ max is related to the square of the amplitude I of the square-wave current, the square-wave period T, and the moment of inertia J of the motor. In order to keep the motor in a quasi-stationary state during the current injection process, the size of the maximum displacement angle θ div is restricted so that the motor rotor only rotates with a small amplitude. max

[0011] In the square-wave current injection calculation, since various parameters of the motor may be unknown, the values of the amplitude I of the square-wave current div and the square-wave period T are determined by an open-loop trial method. The square-wave period T is set to a relatively small fixed value, and the amplitude I of the square-wave currentdiv Gradually increase in each cycle until the rotor crosses the nearest encoder scale line, and the incremental encoder outputs several signals to determine the appropriate square-wave current amplitude I div 。

[0012] The injection angle of the injected square-wave current i ds is two injection angles θ1 and θ2 with a phase difference of 90° electrical angle. In odd cycles, the angle θ1 is selected for injection, and in even cycles, the angle θ2 is selected for injection. This injection strategy can avoid the situation where the injected current does not output torque when a single injection angle coincides with the initial position angle of the rotor, i.e., the positive direction of the d-axis, and ensure that a square-wave current can be injected in at least one direction to output a sufficient torque to generate a position signal.

[0013] In the position identification algorithm module, the pole detection at the input end receives the position signal, the output end of the pole detection is connected to the calculation of the position signal count value, the position signal count value calculation outputs the count value N to the position signal closed-loop controller, and the position signal closed-loop controller outputs the angle signal Δθ to the current control; the position signal closed-loop controller uses the discretized position PI control, i.e., proportional-integral control.

[0014] The position signal is an AB-phase signal with a quarter-cycle phase difference output by the incremental position encoder captured due to the injection of the square-wave current causing several micro-motions with the maximum resolution of the encoder at the output end of the motor reducer.

[0015] The pole detection is realized by using the position signal generated when injecting the square-wave current and cooperating with the encoder interface function in the main control chip; according to the increase and decrease of the count value corresponding to the position signal in the encoder interface, the rotation direction of the robot joint motor is judged, and then the range where the N pole of the rotor is located is determined; the pole detection is realized during the current injection process.

[0016] The position identification algorithm module starts to run after determining the square-wave amplitude, period, and the range where the N pole of the permanent magnet is located; the position signal count value calculation first calculates the difference between the count values of the encoder interface at the start time and T / 2 time of each cycle, i.e., when the rotor is at the maximum displacement angle, and this value is used to characterize the rotation amplitude of the rotor within one cycle. The count value N is input as a feedback quantity into the position signal closed-loop controller, and after accumulation, the injection angle θ in and the estimated value of the difference from the initial position angle θ d of the rotor are output, and the injection angle is gradually changed to coincide with the initial position of the rotor. At this time, the motor hardly outputs torque, the rotation range of the rotor is less than the range of one encoder maximum resolution, and the feedback quantity N stabilizes at the given target value of 0, realizing the identification of the initial position of the robot joint motor rotor.

[0017] Advantages: Compared with the prior art, the initial position identification method of the present invention limits the maximum rotation angle of the rotor and is a detection scheme in a quasi-static state. The present invention can be realized only by the motor output torque, so it does not rely on complex hardware and the salient pole property of the motor, and the identification accuracy is not affected by the salient pole property of the motor, and is applicable to most robot joints using non-salient pole motors. The present invention can also determine the parameters of the injected current appropriately when the motor parameters are unknown, and has a wide range of applicability. In addition, the present invention also realizes pole discrimination during the identification process, making the steps more concise. Brief Description of the Drawings

[0018] Figure 1 is the system composition and control block diagram of the present invention.

[0019] The figure includes: motor driver 1, joint motor 2, reducer 3, incremental position sensor 4, position signal 4.1, motor controller 5, current injection algorithm module 6, position identification algorithm module 7, square wave current injection calculation 6.2, square wave current i ds 6.1, injection angles θ1 and θ2 6.3, coordinate transformation 6.4, SVPWM 6.5, current control 6.6, pole detection 7.1, position signal count value calculation 7.2, count value N 7.3, position signal closed-loop controller 7.4. Detailed Embodiment

[0020] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described in conjunction with the accompanying drawings in the embodiments of the present invention.

[0021] Figure 1 Describes the system composition and control block diagram of the present invention. The robot joint includes a motor driver 1, a joint motor 2, a reducer output 3, an incremental position sensor 4, a motor controller 5, and a position identification algorithm module 6 and a current injection algorithm module 7 running in its main control chip. An incremental encoder is installed at the end of the reducer, and the output position signal is sent to the position identification algorithm module. The position identification algorithm module outputs information such as the angle, amplitude, and period of the injected square wave current to the current injection algorithm module. The current injection module outputs a PWM signal to the motor driver through coordinate transformation 6.4 and SVPWM calculation 6.5. The above-mentioned modules can realize the identification of the initial position angle of the robot joint motor and pole detection. Among them, the motor is usually a surface-mounted permanent magnet synchronous motor, the encoder is generally an incremental photoelectric encoder, and the main control chip can adopt an ARM 32-bit controller such as the STM32F407 chip.

[0022] The injected square wave current i ds6.1 The waveform within one period is a square wave that leads by a quarter of a period, i.e., a waveform of positive, negative, negative, positive. After injecting this square-wave current for one period, the rotor of the robot joint motor will rotate forward by the maximum displacement angle θ max mechanical angle in the first half of the period, and will rotate backward by the same angle and return to the original position in the second half of the period. At the same time, the rotational speed of the motor also decreases to 0. The specific definition of the square-wave current i ds is as follows:

[0023]

[0024] where I div is the amplitude of the square-wave current; T is the period of the current square-wave signal i ds , and t is the injection time of the square-wave current i ds . Generally, t does not exceed 0.5 seconds. After the square-wave current is injected, it is usually sampled using a sampling resistor or a current sensor in cooperation with the ADC peripheral of the main control chip.

[0025] The maximum displacement angle θ max is related to motor parameters such as the square of the square-wave amplitude I div , the square-wave period T, and the moment of inertia J of the motor. In order to keep the motor in a quasi-stationary state during the current injection process, the size of θ max should be restricted so that the motor rotor only rotates with a small amplitude.

[0026] In the square-wave current calculation module 6.2, since various motor parameters may be unknown, the values of I div and T are determined using an open-loop trial-and-error method. The period T is set to a small fixed value. In a specific implementation, T = 0.005 s can be taken. The amplitude I div gradually increases in each period until the rotor crosses the nearest encoder scale line, and the incremental encoder outputs several signals, thereby determining the appropriate amplitude I div , such as 5 A.

[0027] The injection angles of the injected square-wave current are two angles θ1 and θ2 that differ by 90° electrical angle 6.3. In odd periods, angle θ1 is selected for injection, and in even periods, angle θ2 is selected for injection. This injection strategy can avoid the situation where when a single injection angle coincides with the initial position angle of the rotor, i.e., the positive direction of the d-axis, the injected current does not output torque, ensuring that in at least one direction, the injected square-wave current can output a large enough torque to generate a position signal. In the implementation process, θ1 can be arbitrarily selected. For example, if θ1 is 0°, then θ2 is 90°.

[0028] The pole detection method 7.1 is implemented by using the position signal generated when a square-wave current is injected and coordinating with the encoder interface function in the main control chip. According to the increase or decrease of the position signal corresponding to the count value in the encoder interface, the rotation direction of the robot joint motor is judged, and then the range where the N pole of the rotor is located is determined. The pole detection can be realized during the current injection process without additional steps. If the counter increases when injecting in the θ1 direction, the range where the N pole is located is the 180° electrical angle range of [θ1 - π, θ1].

[0029] The position signal 4.1 is the AB-phase signal with a quarter-cycle phase difference output by the incremental position encoder captured due to the fact that the injection of the square-wave current causes several micro-motions of the maximum resolution of the encoder at the output end of the motor reducer. This signal is generally captured and processed by the TIM timer peripheral in the main control chip.

[0030] For the initial position identification algorithm module and the current injection module, the closed-loop controllers therein both use discrete position-type PI control, that is, proportional-integral control. The current injection module also includes functions such as current control, coordinate transformation and inverse transformation, SVPWM, etc., which can be implemented using the open-source code of companies such as STM, and the control period is generally 10kHz - 20kHz.

[0031] The initial position identification algorithm module starts to run after determining the square-wave amplitude, period, and the range where the N pole of the permanent magnet is located. The count value calculation module 7.2 first calculates the difference N 7.3 between the count value of the encoder interface at the start of each period and the T / 2 moment, that is, when the rotor is at the maximum displacement angle. This value is used to characterize the rotation amplitude of the rotor within one period. Taking N as the feedback quantity and inputting it into the closed-loop controller 7.4 in the algorithm, after accumulation, the injection angle θ in and the initial position angle θ of the rotor d the estimated value of the difference are obtained and the injection angle is gradually changed. In the specific implementation process, the injection angle of the current finally coincides with the initial position of the rotor. At this time, the motor hardly outputs torque, the rotation range of the rotor is less than the range of the maximum resolution of one encoder, and the feedback quantity N stabilizes at the given target value of 0, realizing the identification of the initial position of the robot joint motor rotor.

Claims

1. An initial position identification method for a single-incremental position sensor robot joint motor, characterized in that: The robot joint includes a motor driver (1), a robot joint motor (2), a reducer (3), an incremental position sensor (4), a motor controller (5), and a current injection algorithm module (6) and a position identification algorithm module (7) operating in its main control chip. Among them, the motor driver (1), the robot joint motor (2), the reducer (3), and the incremental position sensor (4) are sequentially connected in series. The incremental position sensor (4) outputs a position signal (4.1) to the position identification algorithm module (7). The position identification algorithm module (7) outputs the angle information of the injected square-wave current to the current injection algorithm module (6). The current injection algorithm module (6) calculates the amplitude and period and outputs a PWM signal to the motor driver (1). The motor controller (5) controls the current injection algorithm module (6) and the position identification algorithm module (7) respectively to realize the identification of the initial position angle of the robot joint motor and the pole detection.

2. The initial position identification method for the single-incremental position sensor robot joint motor according to claim 1, characterized in that: In the current injection algorithm module (6), the square wave current injection calculation (6.2) outputs a square wave current i ds (6.1) to current control (6.6), the injection angles θ1 and θ2 (6.3) are sent to current control (6.6) through a switching switch. The output of current control (6.6) is connected to coordinate transformation (6.4), the output of coordinate transformation (6.4) is connected to SVPWM (6.5), and SVPWM (6.5) outputs a PWM signal.

3. The initial position identification method for the single-incremental position sensor robot joint motor according to claim 2, characterized in that: The square-wave current i ds (6.1) The waveform within one period is a square wave that is a quarter-period ahead, i.e., a positive, negative, negative, positive waveform. After injecting the square-wave current for one period, the rotor of the robot joint motor will rotate forward by the maximum displacement angle θ max mechanical angle in the first half of the period, and will rotate backward by the same angle and return to the original position in the second half of the period. At the same time, the rotational speed of the motor also decreases to 0; The square-wave current i ds is specifically defined as follows: where I div is the amplitude of the square-wave current; T is the square wave period of the current signal i ds and t is the injection time of the square wave current i ds .

4. The initial position identification method for the single-increment type position sensor robot joint motor according to claim 3, characterized in that: The maximum displacement angle θ max is related to the square of the square-wave current amplitude I div , the square-wave period T, and the moment of inertia J of the motor. To keep the motor in a quasi-stationary state during current injection, the magnitude of the maximum displacement angle θ max is restricted so that the motor rotor only rotates with a small amplitude.

5. The method for identifying the initial position of the single-incremental position sensor robot joint motor according to claim 2, characterized in that: In the above square-wave current injection calculation (6.2), since various motor parameters may be unknown, the amplitude I of the square-wave current div and the value of the square-wave period T are determined using an open-loop trial-and-error method. The square-wave period T is set to a relatively small fixed value, and the amplitude I of the square-wave current div is gradually increased in each period until the rotor crosses the nearest encoder scale line, and the incremental encoder outputs several signals, thereby determining the appropriate amplitude I of the square-wave current div .

6. The initial position identification method for the single-increment type position sensor robot joint motor according to claim 2, wherein: The injected square-wave current i ds has injection angles of two injection angles θ1 and θ2 (6.3) that differ by 90° electrical angle. In odd cycles, angle θ1 is selected for injection, and in even cycles, angle θ2 is selected for injection. This injection strategy can avoid the situation where no torque is output when the injected current coincides with the initial rotor position angle, i.e., the positive direction of the d-axis, and ensure that a square-wave current can be injected in at least one direction to output a sufficiently large torque to generate a position signal.

7. The method for identifying the initial position of the single-incremental position sensor robot joint motor according to claim 1, characterized in that: In the position identification algorithm module (7), the pole detection (7.1) at its input end is connected to the position signal (4.1). The output end of the pole detection (7.1) is connected to the calculation of the position signal count value (7.2). The calculation of the position signal count value (7.2) outputs a count value N (7.3) to the position signal closed-loop controller (7.4). The position signal closed-loop controller (7.4) outputs an angle signal Δθ to the current control (6.6). The position signal closed-loop controller (7.4) therein uses discretized positional PI control, that is, proportional-integral control.

8. The method for identifying the initial position of the single-incremental position sensor robot joint motor according to claim 7, characterized in that: The position signal (4.1) is an AB-phase signal with a quarter-cycle phase difference, which is output after the incremental position encoder captures several micro-motions of the maximum resolution of the encoder generated at the output end of the motor reducer due to the injection of square-wave current.

9. The initial position identification method for the single-incremental position sensor robot joint motor according to claim 7, characterized in that: The pole detection (7.1) is realized by using the position signal generated when injecting square-wave current and cooperating with the encoder interface function in the main control chip. According to the increase or decrease of the count value corresponding to the position signal in the encoder interface, the rotation direction of the robot joint motor is judged, and then the range where the rotor N pole is located is determined. The pole detection is realized during the current injection process.

10. The method for identifying the initial position of the single-incremental position sensor robot joint motor according to claim 7, wherein: The position identification algorithm module (7) starts running after determining the square wave amplitude, period, and the range where the N pole of the permanent magnet is located; the position signal count value calculation (7.2) first calculates the difference between the encoder interface count values at the start of each period and at T / 2, that is, when the rotor is at the maximum displacement angle, and this value is used to characterize the rotation amplitude of the rotor within one period; the count value N (7.3) is input as a feedback quantity into the position signal closed-loop controller (7.4), and after accumulation, the injection angle θ is output. in and the initial position angle θ of the rotor d The estimated value of the difference and gradually change the injection angle to coincide with the initial position of the rotor. At this time, the motor hardly outputs torque, the rotation range of the rotor is less than the range of one encoder maximum resolution, and the feedback quantity N stabilizes at the given target value of 0, realizing the identification of the initial position of the robot joint motor rotor.

Images