A servo joint lightweight speed observation method and system based on an absolute value encoder and a robot joint servo driver
By adopting a lightweight speed observation method based on absolute encoders, and using conditional judgment and PI controller to generate predicted angles, the problem of unstable speed observation accuracy in the prior art is solved, and high-precision speed feedback and servo system performance improvement are achieved in the full speed domain.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- PANASONIC WELDING SYST TANGSHAN
- Filing Date
- 2026-03-24
- Publication Date
- 2026-06-26
Smart Images
Figure CN122284424A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to a lightweight speed observation method, system, and robot joint servo driver based on an absolute encoder, belonging to the field of servo control technology. Background Technology
[0002] Permanent magnet synchronous motor servo systems are commonly used in high-performance control applications such as industrial robots and CNC machine tools. Robot joint servo drives typically use absolute encoders to obtain rotor position information and obtain speed feedback signals through speed observers for closed-loop speed control.
[0003] Currently, the commonly used velocity observation methods in engineering practice mainly include the following: M-method speed measurement: This method calculates speed by counting the number of encoder pulses within a fixed sampling period. While this method is highly accurate at high speeds, its accuracy drops sharply at low speeds due to the smaller number of pulses within the sampling period, resulting in noticeable step-like fluctuations in speed feedback. Furthermore, the M-method, being essentially a position differential method, amplifies encoder quantization noise, leading to numerous speed feedback glitches.
[0004] T-method speed measurement: This method calculates speed by measuring the time interval between two adjacent encoder pulses. It is highly accurate at low speeds, but at high speeds, the measurement time becomes too short, causing a decrease in accuracy. It is also relatively complex to implement with absolute encoders.
[0005] Traditional phase-locked loop (PLL) speed observation: The position error is calculated using the arctangent function, and the speed is estimated by a PI controller. This method can obtain smooth speed feedback, but it has the following drawbacks: ① The arctangent function has a large computational load, consuming DSP resources. Even with a hardware acceleration unit (such as a TMU), it still requires 10-15 instruction cycles; ② The observation angle is only used for speed feedback and is not fully utilized for current loop control; ③ It is generally used for sensorless control.
[0006] Direct differential + low-pass filtering: A low-pass filter is cascaded after the position differential. Although this method is simple to implement, the low-pass filtering introduces phase lag, which affects the dynamic response performance of the velocity loop, and the lag is positively correlated with the filter strength. Summary of the Invention
[0007] The purpose of this invention is to provide a lightweight speed observation method, system, and robot joint servo driver based on an absolute encoder, which can achieve high-precision speed observation with low computational cost, and at the same time provide predicted angles for current loop control (inverse Park transformation), thereby improving the overall performance of the servo system.
[0008] To achieve the above-mentioned technical effects, the present invention is implemented using the following technical solution.
[0009] In a first aspect, the present invention provides a lightweight speed observation method for servo joints based on an absolute encoder, comprising: Set the initial values for the phase-locked loop to estimate the position and velocity observations; Based on each control cycle, the current actual position output by the absolute encoder is obtained; The position error is obtained by normalizing the difference between the current actual position and the position estimated by the phase-locked loop in the previous control cycle. The position error is input to the PI controller to obtain the speed correction amount; Update the speed observation value for the current control cycle based on the speed correction amount and the speed observation value of the previous control cycle. The velocity observation value of the current control cycle is integrated and normalized to obtain the estimated position of the current control cycle; The speed observation value of the current control cycle is output as a speed feedback signal, and the estimated position of the current control cycle is used as the estimated position of the phase-locked loop for the next control cycle.
[0010] In conjunction with the first aspect, further, the position error is obtained by normalizing the difference between the current actual position and the position estimated by the phase-locked loop in the previous control cycle, including: Calculate the original difference between the current actual position and the estimated position of the phase-locked loop in the previous control cycle. ; For the original difference Angle normalization is performed to make the original difference... The range of values is limited to ; The normalized original difference is used as the final position error.
[0011] In conjunction with the first aspect, further, the original difference is made The range of values is limited to ,include: The original difference is evaluated using a judgment logic. Angle normalization is performed, specifically as follows: when Then let until ; when Then let until .
[0012] In conjunction with the first aspect, the expression for the speed correction amount is further as follows: ; ; in, This represents the discrete integral term of the PI controller; This represents the discrete integral term of the PI controller in the previous control cycle. Indicates the integration parameter; This represents the original difference between the current actual position and the estimated position from the phase-locked loop in the previous control cycle; Indicates the speed correction amount; This represents the discrete proportional parameter of the PI controller.
[0013] In conjunction with the first aspect, further, updating the velocity observation value of the current control cycle includes: The speed correction output from the PI controller is accumulated into the speed observation value of the previous control cycle to update the speed observation value of the current control cycle. The expression for the velocity observation value is: ; in, This represents the velocity observation value for the current control cycle; This represents the velocity observation value from the previous control cycle; This indicates the speed correction amount.
[0014] In conjunction with the first aspect, the expression for the estimated position of the current control cycle is further as follows: ; in, Indicates the estimated position of the current control cycle; Indicates the estimated position of the previous control cycle; This represents the velocity observation value for the current control cycle; Indicates the time factor.
[0015] In conjunction with the first aspect, it further includes: Based on the speed observation value of the current control cycle and the estimated position of the current control cycle Perform magnetic field angle prediction to generate a predicted angle to compensate for digital control delay; Normalize the predicted angle to Within the range, and converted into voltage commands through inverse Park transformation; The expression for generating the predicted angle is: ; in, Indicates the prediction angle; Indicates the estimated position of the current control cycle; This represents the velocity observation value for the current control cycle; Indicates the time factor; This indicates the number of prediction steps.
[0016] Secondly, a lightweight speed monitoring system for servo joints based on an absolute encoder includes: The initialization module is used to set the estimated position and velocity observations of the phase-locked loop during the initial control cycle; The encoder interface module is used to obtain the current actual position output by the absolute encoder based on each control cycle; The error calculation module is used to normalize the difference between the current actual position and the position estimated by the phase-locked loop in the previous control cycle to obtain the position error. The PI controller module is used to input the position error to the PI controller to obtain the speed correction amount; The speed observation module is used to update the speed observation value of the current control cycle based on the speed correction amount and the speed observation value of the previous control cycle. The position observation module is used to integrate and normalize the velocity observation value of the current control cycle to obtain the estimated position of the current control cycle. The output module is used to output the speed observation value of the current control cycle as a speed feedback signal, and to use the estimated position of the current control cycle as the estimated position of the phase-locked loop for the next control cycle.
[0017] In conjunction with the second aspect, it further includes an angle prediction module, which is used to predict the speed based on the velocity observation value of the current control cycle. and the estimated position of the current control cycle Perform magnetic field angle prediction to generate a predicted angle to compensate for digital control delay, and convert the predicted angle into a voltage command through inverse Park transformation.
[0018] Thirdly, a robot joint servo actuator includes the lightweight speed observation system for servo joints based on an absolute encoder as described in the second aspect.
[0019] Compared with the prior art, the beneficial effects achieved by the present invention are as follows: This invention uses conditional judgment to replace the arctangent function in the traditional phase-locked loop for angle normalization. The calculation amount of a single control cycle does not exceed 4 instruction cycles, which significantly reduces the DSP resource occupation and realizes lightweight speed observation. By using the phase-locked loop structure to adjust the position error with PI, it avoids the step-like fluctuations that occur in the M-method speed measurement at low speeds and the phase lag problem introduced by direct differential plus low-pass filtering, and obtains smooth, continuous and high-precision speed feedback in the entire speed domain.
[0020] Furthermore, this invention utilizes the observed speed and position information to generate a predicted angle, compensating for the digital control delay caused by inverter output delay and encoder reading cycle, and deeply integrates speed observation with current loop control, thereby improving the dynamic response performance and control accuracy of the servo system. Attached Figure Description
[0021] Figure 1 The figure shown is a system block diagram of a lightweight speed observation method for servo joints based on an absolute encoder provided in an embodiment of the present invention. Figure 2 The diagram shows a flowchart of a lightweight speed observation method for servo joints based on an absolute encoder, provided in an embodiment of the present invention. Detailed Implementation
[0022] The technical solution of the present invention will be described in detail below with reference to the accompanying drawings and specific embodiments. It should be understood that the embodiments of the present invention and the specific features in the embodiments are detailed descriptions of the technical solution of the present invention, rather than limitations thereof. In the absence of conflict, the embodiments of the present invention and the technical features in the embodiments can be combined with each other. Example 1
[0023] See Figure 2 This embodiment introduces a lightweight speed observation method for servo joints based on an absolute encoder, including the following steps: Step 1: Set the phase-locked loop to estimate the position and velocity observations for the initial control cycle; To ensure stable convergence of the phase-locked loop (PLL), the estimated position needs to be determined before startup. Compared with velocity observations During initialization, the estimated position can typically be set to the first reading value of the absolute encoder, and the velocity observation value can be set to zero.
[0024] Step 2: Obtain the current actual position output by the absolute encoder. ; Specifically, based on each control cycle, the current actual position is acquired from the output of a high-resolution absolute encoder (such as the BiSS-C protocol, 19 bits or higher resolution). And read the PLL estimated position from the previous control cycle. .
[0025] Step 3: Normalize the difference between the current actual position and the position estimated by the phase-locked loop in the previous control cycle to obtain the position error; The core of this step lies in replacing the arctangent function in the traditional phase-locked loop with conditional judgments, which significantly reduces the amount of computation. Specifically, it includes the following sub-steps: Step 31: Calculate the current actual position Position estimated by the phase-locked loop in the previous control cycle raw difference Its expression is: (1) in, Indicates the original difference; This indicates the current actual position output by the absolute encoder; This indicates the estimated position of the PLL in the previous control cycle.
[0026] Traditional PLLs use the arctangent function to calculate phase difference, which still requires 10-15 instruction cycles even in DSPs with hardware acceleration units, resulting in high computational load and resource consumption. Therefore, this invention uses the conditional judgment described in step 32 below to replace the arctangent function for normalization processing, so that the computational load of a single control cycle does not exceed 4 instruction cycles (for a DSP with a main frequency of 200MHz), achieving lightweight observation.
[0027] Step 32: Use judgment logic to evaluate the original difference. Perform angle normalization to make the original difference The range of values is limited to The details are as follows: a. When Then let until ; b. When Then let until .
[0028] Step 33: Use the normalized original difference as the final position error.
[0029] By replacing complex trigonometric functions with simple addition and subtraction operations, the accuracy of angular errors is ensured, while greatly improving computational efficiency.
[0030] Step 4: Input the position error into the PI controller to obtain the speed correction value, the expression of which is: (2) (3) in, This represents the discrete integral term of the PI controller; This represents the discrete integral term of the PI controller in the previous control cycle. Indicates the integration parameter; Indicates the original difference; Indicates the speed correction amount; This represents the discrete proportional parameter of the PI controller.
[0031] It should be noted that the parameters of the PI controller are tuned using the bandwidth method, as detailed below: Determine the bandwidth of the phase-locked loop based on the system bandwidth requirements. Damping ratio (Preferred) ); Based on the bandwidth of the PLL The relationship with the natural frequency, when the damping ratio When calculating the natural frequency Thus, the continuous domain parameters are obtained. , ; Discretize the parameters in the continuous domain to obtain , .
[0032] This method enables PPL to maintain excellent dynamic response and stability within the desired bandwidth.
[0033] Step 5: Update the speed observation value for the current control cycle based on the speed correction amount and the speed observation value from the previous control cycle; This velocity observation replaces the traditional M-method or differential filtering method, avoiding the stepped fluctuations and phase lag problems at low speeds, and achieving smooth and continuous velocity feedback across the entire velocity domain. Its expression is: (4) in, This represents the velocity observation value for the current control cycle; This represents the velocity observation value from the previous control cycle; This indicates the speed correction amount.
[0034] Step 6: Integrate the speed observation value of the current control cycle using an integrator, and normalize the integrated speed observation value to obtain the estimated position of the current control cycle. Through the integration step, the PLL achieves smooth tracking of the actual position, expressed as: (5) in, Indicates the estimated position of the current control cycle; Indicates the estimated position of the previous control cycle; Indicates the time factor.
[0035] Step 7: Output the speed observation value of the current control cycle as the speed feedback signal, and use the estimated position of the current control cycle as the estimated position of the phase-locked loop for the next control cycle; Normalize the estimated location to Within the scope, ensure It always stays within the valid range to prevent numerical overflow and lays the foundation for error calculation in the next cycle.
[0036] Step 8: Velocity observations based on the current control cycle and the estimated position of the current control cycle Perform magnetic field angle prediction to generate a predicted angle to compensate for digital control delay.
[0037] Since the speed observation value in the traditional method only serves the speed loop, this step actively compensates for the delay of the digital control system by predicting the angle, improves the control accuracy of the current loop, and enables the servo system to maintain excellent control performance in high dynamic response scenarios.
[0038] The expression for the predicted angle is: (6) in, Indicates the prediction angle; This indicates the prediction step count, used to compensate for the digital control delay caused by the inverter output delay and the encoder reading cycle. It is generally taken as 1 to 3.
[0039] In summary, this invention achieves error calculation by replacing arctangent with conditional judgment, tunes PI control parameters using the bandwidth method, and generates predicted angles using observed values. This significantly reduces computational overhead while ensuring the accuracy of speed observation, and achieves deep integration of speed observation and current loop control, effectively improving the overall performance of robot joint servo drive. Example 2
[0040] See Figure 1 A lightweight speed monitoring system for servo joints based on an absolute encoder, comprising: The initialization module is used to set the initial values for the position and velocity observations estimated by the phase-locked loop. The encoder interface module is used to obtain the current actual position output by the absolute encoder based on each control cycle; The error calculation module is used to normalize the difference between the current actual position and the position estimated by the phase-locked loop in the previous control cycle to obtain the position error. The PI controller module is used to input the position error to the PI controller to obtain the speed correction amount; The speed observation module is used to update the speed observation value of the current control cycle based on the speed correction amount and the speed observation value of the previous control cycle. The position observation module is used to integrate and normalize the velocity observation values of the current control cycle to obtain the estimated position of the current control cycle. The output module is used to output the speed observation value of the current control cycle as a speed feedback signal, and to use the estimated position of the current control cycle as the estimated position of the phase-locked loop for the next control cycle. The angle prediction module is used to predict the magnetic field angle based on the speed observation value and the estimated position of the current control cycle, generate a predicted angle to supplement the digital control delay, and convert the predicted angle into a voltage command through inverse Park transformation.
[0041] The three-phase inverter receives the voltage command output from the inverse Park converter. and It drives the servo joint motor to operate, and the servo joint motor achieves closed-loop control of position and speed under the drive of the three-phase inverter. Example 3
[0042] A robot joint servo actuator includes the lightweight speed observation system for servo joints based on an absolute encoder as described in Example 2.
[0043] The embodiments of the present invention have been described above with reference to the accompanying drawings. However, the present invention is not limited to the specific embodiments described above. The specific embodiments described above are merely illustrative and not restrictive. Those skilled in the art can make many other forms under the guidance of the present invention without departing from the spirit and scope of the claims. All of these forms are within the protection scope of the present invention.
Claims
1. A lightweight speed observation method for servo joints based on absolute encoders, characterized in that, include: Set the initial values for the phase-locked loop to estimate the position and velocity observations; Based on each control cycle, the current actual position output by the absolute encoder is obtained; The position error is obtained by normalizing the difference between the current actual position and the position estimated by the phase-locked loop in the previous control cycle. The position error is input to the PI controller to obtain the speed correction amount; Update the speed observation value for the current control cycle based on the speed correction amount and the speed observation value of the previous control cycle. The velocity observation value of the current control cycle is integrated and normalized to obtain the estimated position of the current control cycle; The speed observation value of the current control cycle is output as a speed feedback signal, and the estimated position of the current control cycle is used as the estimated position of the phase-locked loop for the next control cycle.
2. The lightweight speed observation method for servo joints based on absolute encoders according to claim 1, characterized in that, The position error is obtained by normalizing the difference between the current actual position and the position estimated by the phase-locked loop in the previous control cycle, including: Calculate the original difference between the current actual position and the estimated position of the phase-locked loop in the previous control cycle. ; For the original difference Angle normalization is performed to make the original difference... The range of values is limited to ; The normalized original difference is used as the final position error.
3. The lightweight speed observation method for servo joints based on absolute encoders according to claim 2, characterized in that, So that the original difference The range of values is limited to ,include: The original difference is evaluated using a judgment logic. Angle normalization is performed, specifically as follows: when Then let until ; when Then let until .
4. The lightweight speed observation method for servo joints based on absolute encoders according to claim 1, characterized in that, The expression for the speed correction amount is: ； ； in, This represents the discrete integral term of the PI controller; This represents the discrete integral term of the PI controller in the previous control cycle. Indicates the integration parameter; This represents the original difference between the current actual position and the estimated position from the phase-locked loop in the previous control cycle; Indicates the speed correction amount; This represents the discrete proportional parameter of the PI controller.
5. The lightweight speed observation method for servo joints based on absolute encoders according to claim 1, characterized in that, The updating of the velocity observation value for the current control cycle includes: The speed correction output from the PI controller is accumulated into the speed observation value of the previous control cycle to update the speed observation value of the current control cycle. The expression for the velocity observation value is: ； in, This represents the velocity observation value for the current control cycle; This represents the velocity observation value from the previous control cycle; This indicates the speed correction amount.
6. The lightweight speed observation method for servo joints based on absolute encoders according to claim 1, characterized in that, The expression for the estimated position of the current control cycle is: ； in, Indicates the estimated position of the current control cycle; Indicates the estimated position of the previous control cycle; This represents the velocity observation value for the current control cycle; Indicates the time factor.
7. The lightweight speed observation method for servo joints based on absolute encoders according to claim 1, characterized in that, Also includes: Based on the speed observation value of the current control cycle and the estimated position of the current control cycle Perform magnetic field angle prediction to generate a predicted angle to compensate for digital control delay; Normalize the predicted angle to Within the range, and converted into voltage commands through inverse Park transformation; The expression for generating the predicted angle is: ； in, Indicates the prediction angle; Indicates the estimated position of the current control cycle; This represents the velocity observation value for the current control cycle; Indicates the time factor; This indicates the number of prediction steps.
8. A lightweight speed monitoring system for servo joints based on an absolute encoder, characterized in that, include: The initialization module is used to set the initial values for the position and velocity observations estimated by the phase-locked loop. The encoder interface module is used to obtain the current actual position output by the absolute encoder based on each control cycle; The error calculation module is used to normalize the difference between the current actual position and the position estimated by the phase-locked loop in the previous control cycle to obtain the position error. The PI controller module is used to input the position error to the PI controller to obtain the speed correction amount; The speed observation module is used to update the speed observation value of the current control cycle based on the speed correction amount and the speed observation value of the previous control cycle. The position observation module is used to integrate and normalize the velocity observation value of the current control cycle to obtain the estimated position of the current control cycle. The output module is used to output the speed observation value of the current control cycle as a speed feedback signal, and to use the estimated position of the current control cycle as the estimated position of the phase-locked loop for the next control cycle.
9. The lightweight speed observation system for servo joints based on an absolute encoder according to claim 8, characterized in that, It also includes an angle prediction module, which is used to predict the speed based on the speed observation value of the current control cycle. and the estimated position of the current control cycle Perform magnetic field angle prediction to generate a predicted angle to compensate for digital control delay, and convert the predicted angle into a voltage command through inverse Park transformation.
10. A robot joint servo driver, characterized in that, Includes the lightweight speed observation system for servo joints based on an absolute encoder as described in any one of claims 8-9.