A data processing-based servo motor control method and system

Abstract

The application belongs to the technical field of robot control, and discloses a servo motor control method and system based on data processing, which comprises the following steps: direction sensing accumulation is performed on an angle signal based on a direction of an angular velocity signal, a stroke phase and an effective marker are obtained; current effective samples corresponding to the effective marker are rearranged according to the stroke phase to obtain a phase domain signal and calculate a quality index; vibration amplitudes of each target order are extracted; an order vibration intensity index is generated based on the vibration amplitudes, the quality index and a reference amplitude parameter; when a direction of the angular velocity is detected to be reversed, a reverse impact risk level is determined according to a vibration energy change amount before and after the reversal and the quality index; and a torque instruction is shaped and output based on the order vibration intensity index and the reverse impact risk level. Thus, the center frequency of the notch filter can track the meshing excitation frequency in real time with the rotating speed, and the vibration suppression parameter can be dynamically adjusted according to the vibration intensity, so that effective vibration suppression can be maintained under the conditions of varying rotating speed and load.

Description

Technical Field

[0001] This invention relates to the field of robot control technology, and more specifically, to a servo motor control method and system based on data processing. Background Technology

[0002] Servo motors and reducers are widely used in industrial robots for their joint structures. During joint movement, the periodic meshing of gears within the reducer generates periodic excitation in the transmission chain. This excitation, superimposed on the joint movement, can cause increased joint vibration and position tracking errors under certain speeds or load conditions. To address this, existing control systems typically test the main vibration frequencies of the joint during the commissioning phase and suppress them using a notch filter with fixed parameters in the servo controller. However, the frequency of gear meshing excitation is directly related to the motor speed, and the excitation frequency changes as the joint operates at varying speeds. Since the parameters of the notch filter remain unchanged after commissioning, it can only effectively suppress vibrations at specific speeds. In actual operation, the vibration suppression effect deteriorates when the speed deviates from the commissioning conditions.

[0003] When the joint switches between forward and reverse rotation, the backlash in the reducer's transmission chain causes the tooth surfaces to undergo a process of disengagement and re-engagement. This process generates a brief disturbance in the torque output, which manifests as error fluctuations near the switching point on the position curve. Existing controllers typically trigger preset torque transition processing when the speed crosses zero. However, the strength of the disturbance during switching depends on factors such as speed and load at the moment of switching, and varies significantly under different operating conditions. A single set of fixed processing parameters is difficult to accommodate all situations; it may be insufficient to suppress strong impacts, while it may introduce unnecessary response lag when the impact is weak. Summary of the Invention

[0004] To overcome the aforementioned problems in the prior art, this invention proposes a servo motor control method and system based on data processing to solve the above problems.

[0005] This invention provides the following technical solution: A servo motor control method based on data processing includes: Acquire the angle and angular velocity signals of the servo motor, perform directional sensing accumulation on the angle signal based on the direction of the angular velocity signal, and obtain the stroke phase and an effective marker to distinguish the accumulation state from the stop accumulation state. The current signal of the servo motor is acquired, and valid samples corresponding to valid markers are selected from the current signal. The valid samples are rearranged at equal intervals according to the stroke phase to obtain the phase domain signal. The quality index is calculated based on the sufficiency of the number of valid samples and the goodness of fit of the rearrangement. The vibration amplitude of each target order is extracted from the phase domain signal; based on the vibration amplitude, quality index and pre-acquired reference amplitude parameters, the vibration intensity index of each order is generated. When a reversal in the angular velocity direction is detected, the risk level of the reverse impact is determined based on the change in vibration energy before and after the reversal and the quality indicators. The greater the change in vibration energy, the higher the risk level. Based on the order vibration intensity index and the reverse impact risk level, the torque command is shaped and the shaped torque command is output.

[0006] Preferably, the direction sensing accumulation of the angle signal based on the direction of the angular velocity signal includes: Each control cycle calculates the angle increment of the angle signal at adjacent sampling times; Hysteresis judgment with upper and lower switching thresholds is applied to the angular velocity signal, where the upper switching threshold is greater than the lower switching threshold; Hysteresis detection includes: when the absolute value of angular velocity exceeds the upper switching threshold and remains above the upper switching threshold for multiple consecutive control cycles, it is confirmed that the current state is high speed; when the absolute value of angular velocity is lower than the lower switching threshold, it is confirmed that the current state is low speed; when the absolute value of angular velocity is between the upper and lower switching thresholds, the current state remains unchanged. In each control cycle at high speed, the absolute value of the angle increment of this control cycle is accumulated into the stroke phase, and this control cycle is marked as valid. In each control cycle at low speed, the stroke phase remains unchanged, and the current control cycle is marked as invalid.

[0007] Preferably, the step of rearranging the effective samples at equal intervals according to their travel phases to obtain a phase domain signal, and calculating the quality index based on the sufficiency of the number of effective samples and the goodness of fit of the rearrangement, includes: With the goal of an equally spaced phase grid covering the effective sample travel phase range, the effective samples are mapped to each node of the phase grid through interpolation to obtain a phase domain signal that is uniformly distributed in the travel phase domain. The coverage rate component is obtained based on the sufficiency of the number of effective samples relative to the preset minimum number of samples. The phase domain signal is substituted back at the travel phase position of the effective samples, and the goodness of fit component is obtained based on the normalized fitting error between the substituted value and the original effective sample value. The smaller the normalized fitting error, the larger the goodness of fit component. The coverage component and the goodness-of-fit component are combined to obtain the quality index. The larger the coverage component and the goodness-of-fit component, the larger the quality index.

[0008] Preferably, the method for obtaining the reference amplitude parameter includes: The system acquires historically collected current signals, performs order analysis on the current signals according to the stroke phase, extracts vibration amplitude samples for each target order, performs statistical analysis on the vibration amplitude samples, and obtains the baseline mean and baseline dispersion for each target order, which are used as baseline amplitude parameters.

[0009] Preferably, the generation of the order vibration intensity index based on the vibration amplitude, quality index, and pre-acquired reference amplitude parameters includes: For each target order, the deviation of the vibration amplitude from the benchmark mean is measured using the benchmark dispersion as a reference scale. The deviation is then clipped to the non-negative range and the upper limit is restricted to obtain the normalized excess for that order. The normalized excess of each target order is weighted and summed according to preset weights to obtain the weighted vibration intensity; The weighted vibration intensity is monotonically fused with the quality index. The smaller the quality index, the smaller the fusion result, thus obtaining the mass-weighted vibration intensity. By imposing a limit on the maximum change in mass-weighted vibration intensity per control cycle, the order vibration intensity index is obtained.

[0010] Preferably, determining the reverse impact risk level based on the change in vibration energy before and after the overturning and the quality indicators includes: Monitor the sign of the angular velocity signal. When the sign of the angular velocity changes and the new direction is maintained for a subsequent preset number of control cycles, confirm a reverse event. After confirming the reverse event, the energy metric values ​​of each target order vibration amplitude are calculated within each preset time window before the direction reversal and after the direction confirmation is completed, so as to obtain the energy before the reversal and the energy after the reversal. The two time windows have the same length. The risk level of the reverse shock is determined based on the degree to which the energy after the reversal exceeds the energy before the reversal. The greater the degree of excess, the higher the risk level. When the degree of excess does not exceed zero, it corresponds to the lowest risk level. When the quality index is not higher than the preset quality threshold, or when the absolute value of the angular velocity is continuously lower than the preset low speed threshold, the reverse impact risk level will be maintained at a higher risk level; when the quality index recovers to be higher than the preset quality threshold and the absolute value of the angular velocity is no longer continuously lower than the preset low speed threshold, the reverse impact risk level will be determined based on the degree of excess.

[0011] Preferably, the shaping of the torque command based on the order vibration intensity index and the reverse impact risk level includes: Based on the product of the current rotational speed and each target order, the notch center frequency corresponding to each target order is determined. The notch depth is set to a value that increases monotonically and is limited as the vibration intensity index of the order increases. The torque command is subjected to notch filtering with notch center frequency and notch depth as parameters to obtain the notch-filtered torque. The upper limit of the torque change rate is set to a value that decreases monotonically with the increase of the order vibration intensity index and decreases monotonically with the increase of the reverse impact risk level, thereby imposing a torque change rate limit on the torque after the notch wave. When the torque of the current control cycle has the opposite sign to the torque of the previous control cycle and the reverse impact risk level exceeds the preset risk threshold, within the range where the absolute value of the torque is lower than the preset buffer threshold, the upper limit of the torque change rate is replaced with the upper limit of the buffer segment rate, which is smaller than the current upper limit of the torque change rate, and a shaping torque command is output.

[0012] Preferably, setting the notch depth to a value that monotonically increases with increasing order vibration intensity index and is limited includes: The notch depth is limited between a preset minimum notch depth and a preset maximum notch depth, and an upper limit is imposed on the change in notch depth per control cycle, so that the notch depth smoothly transitions to the target value.

[0013] The present invention also provides a servo motor control system based on data processing, for implementing a servo motor control method based on data processing, comprising: The stroke phase construction module is used to acquire the angle signal and angular velocity signal of the servo motor, and to perform direction sensing accumulation on the angle signal based on the direction of the angular velocity signal to obtain the stroke phase and an effective marker to distinguish the accumulation state from the stop accumulation state. The phase domain rearrangement module is used to acquire the current signal of the servo motor, filter the valid samples corresponding to the valid markers from the current signal, rearrange the valid samples at equal intervals according to the stroke phase to obtain the phase domain signal, and calculate the quality index based on the sufficiency of the number of valid samples and the goodness of fit of the rearrangement. The vibration intensity analysis module is used to extract the vibration amplitude of each target order from the phase domain signal; and to generate the order vibration intensity index based on the vibration amplitude, quality index and pre-acquired reference amplitude parameters. The impact risk assessment module is used to determine the reverse impact risk level based on the change in vibration energy before and after the rollover and the quality index when a rollover in the angular velocity direction is detected. The greater the change in vibration energy, the higher the risk level. The torque command shaping module is used to shape the torque command based on the order vibration intensity index and the reverse impact risk level, and output the shaped torque command.

[0014] This invention provides a servo motor control method and system based on data processing, which has the following beneficial effects: This invention, through joint processing of motor current and encoder signals, constructs a stroke phase by accumulating the angle increment in the angular velocity direction. Combined with hysteresis detection, it generates valid markers. The valid current samples are rearranged into phase domain signals according to the stroke phase. The vibration amplitude of each target order is extracted and combined with quality indicators to generate order vibration intensity indicators. Based on this, the center frequency, notch depth, and upper limit of torque change rate of the notch filter are adjusted online. Thus, the center frequency of the notch filter tracks the actual meshing excitation frequency in real time with the rotational speed, and the vibration suppression parameters are dynamically adjusted with the vibration intensity. This maintains effective vibration suppression under varying speed and load conditions, reducing peak tracking error and joint vibration.

[0015] This invention, upon detecting a reversal in the angular velocity direction, determines the risk level of a reverse impact by comparing the relative changes in the target order vibration energy before and after the reversal. When the torque crosses zero and the risk level exceeds a preset threshold, a smaller buffer speed limit is activated within the zero-crossing interval. Therefore, the shaping force for the torque zero-crossing directly depends on the measured change in meshing excitation energy. When the impact is strong, the torque zero-crossing process is slowed down accordingly; when the impact is weak, no additional restrictions are applied. This suppresses meshing impact while avoiding unnecessary response lag caused by a fixed conservative strategy. Attached Figure Description

[0016] Figure 1 This is a flowchart illustrating a servo motor control method based on data processing according to the present invention. Figure 2 This is a schematic diagram of a servo motor control system based on data processing according to the present invention. Detailed Implementation

[0017] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0018] Example 1 Please see Figure 1 In this embodiment, a servo motor control method based on data processing includes: S1. Obtain the angle signal and angular velocity signal of the servo motor. Based on the direction of the angular velocity signal, perform direction sensing accumulation on the angle signal to obtain the stroke phase and an effective marker to distinguish between the accumulation state and the stop accumulation state. The direction sensing and accumulation of the angle signal based on the direction of the angular velocity signal includes: Each control cycle calculates the angle increment of the angle signal at adjacent sampling times; Hysteresis judgment with upper and lower switching thresholds is applied to the angular velocity signal, where the upper switching threshold is greater than the lower switching threshold; Hysteresis detection includes: when the absolute value of angular velocity exceeds the upper switching threshold and remains above the upper switching threshold for multiple consecutive control cycles, it is confirmed that the current state is high speed; when the absolute value of angular velocity is lower than the lower switching threshold, it is confirmed that the current state is low speed; when the absolute value of angular velocity is between the upper and lower switching thresholds, the current state remains unchanged. In each control cycle at high speed, the absolute value of the angle increment of this control cycle is accumulated into the stroke phase, and this control cycle is marked as valid. In each control cycle at low speed, the stroke phase remains unchanged, and the current control cycle is marked as invalid.

[0019] In this embodiment, taking a wrist joint of a six-axis industrial robot as an example, the joint is driven by a servo motor and a harmonic reducer. The controller operates with a control cycle of 1 millisecond. The driver provides the motor-side encoder angle, angular velocity estimation value and q-axis current reading to the controller through the bus. The controller outputs torque commands to the driver. The joint needs to frequently switch between forward and reverse rotation during normal operation.

[0020] It should be noted that the angle signal is the absolute angle value of the encoder on the motor side reported by the driver in each control cycle, in degrees; the angular velocity signal is an estimated value provided by the driver based on the encoder angle difference and after low-pass filtering, in rad / s.

[0021] Each control cycle reads the difference between the current encoder angle and the encoder angle of the previous control cycle to obtain the angle increment for the current cycle, in degrees. This increment is positive during forward rotation, negative during reverse rotation, and close to zero during pauses.

[0022] Hysteresis is applied to the angular velocity signal. In this embodiment, the upper switching threshold is set to 0.5 rad / s, and the lower switching threshold is set to 0.2 rad / s. Two thresholds are used instead of one to prevent the angular velocity from fluctuating frequently around a single threshold due to estimation noise, causing repeated switching between high and low speeds and introducing incorrect travel phase increments. With two thresholds, state switching is triggered only when the angular velocity explicitly crosses each threshold; when it is between the two, the current state remains unchanged.

[0023] Once the absolute value of the angular velocity exceeds 0.5 rad / s, it is required that the absolute value of the angular velocity remain above 0.5 rad / s for the next five consecutive control cycles before the current high-speed state is officially confirmed. This continuous confirmation over five cycles further prevents the joint from being mistakenly judged as having entered stable motion when its speed briefly exceeds the threshold during the initial acceleration or reversal. When the absolute value of the angular velocity is below 0.2 rad / s, the current low-speed state is directly confirmed without continuous confirmation, as the speed is clearly below the lower limit.

[0024] After confirming high-speed operation, each control cycle accumulates the absolute value of the angle increment to the stroke phase. The reason for using the absolute value is that the stroke phase records the cumulative distance traveled by the joint, regardless of direction. The angle increment is positive during forward rotation and negative during reverse rotation; after taking the absolute value, the increment is always positive, and the stroke phase always monotonically increases. For example, if the joint first rotates 30 degrees forward, pauses, and then reverses 20 degrees, the stroke phase accumulates sequentially to 30 degrees, remains at 30 degrees, and continues to accumulate to 50 degrees. However, the actual joint angle first increases by 30 degrees and then decreases by 20 degrees, which is completely different. This monotonically increasing characteristic of the stroke phase allows it to be used as the independent variable for subsequent signal rearrangement, which is key to the ability of this scheme to perform order analysis under reciprocating motion conditions. Simultaneously, the current cycle is marked as valid.

[0025] Once the low-speed state is confirmed, the stroke phase remains unchanged from the previous cycle, and the current cycle is marked as invalid. During the low-speed phase, the motor speed is too low, making angular velocity direction determination unreliable. The current signal during this phase is unsuitable for order analysis and is therefore removed from subsequent processing by marking it as invalid.

[0026] S2. Obtain the current signal of the servo motor, filter the valid samples corresponding to the valid markers from the current signal, rearrange the valid samples at equal intervals according to the stroke phase to obtain the phase domain signal, and calculate the quality index based on the sufficiency of the number of valid samples and the goodness of fit of the rearrangement. The process of rearranging the effective samples at equal intervals according to their travel phase to obtain a phase domain signal, and calculating the quality index based on the sufficiency of the number of effective samples and the goodness of fit of the rearrangement, includes: With the goal of an equally spaced phase grid covering the effective sample travel phase range, the effective samples are mapped to each node of the phase grid through interpolation to obtain a phase domain signal that is uniformly distributed in the travel phase domain. The coverage rate component is obtained based on the sufficiency of the number of effective samples relative to the preset minimum number of samples. The phase domain signal is substituted back at the travel phase position of the effective samples, and the goodness of fit component is obtained based on the normalized fitting error between the substituted value and the original effective sample value. The smaller the normalized fitting error, the larger the goodness of fit component. The coverage component and the goodness-of-fit component are combined to obtain the quality index. The larger the coverage component and the goodness-of-fit component, the larger the quality index.

[0027] In this embodiment, the current signal is the q-axis current reading reported by the driver in each control cycle, in amperes. In vector control, the q-axis current is proportional to the motor output torque, and its fluctuations directly reflect torque fluctuations. Since gear meshing excitation appears in the form of torque fluctuations, the q-axis current is a suitable signal source for extracting meshing vibration characteristics.

[0028] The controller maintains a sliding buffer that continuously stores current sampling points from the most recent control cycles, along with their corresponding travel phase values ​​and valid markers. The buffer length is designed to cover the travel phase range required by a preset phase grid. In this embodiment, the phase grid covers a 180-degree travel, and the buffer retains enough samples to accumulate 180 degrees of valid travel. Sampling points corresponding to valid markers are selected from the buffer. Each valid sample contains the travel phase value and q-axis current value at that moment, forming a valid sample set.

[0029] The purpose of rearranging the valid samples at equal intervals according to their travel phase is to transform the unevenly distributed samples in the travel phase domain into a uniformly distributed sequence, enabling subsequent order analysis to proceed correctly. An equally spaced phase grid is established within a 180-degree range forward from the maximum travel phase value of the valid samples. In this embodiment, the phase grid step size is 0.5 degrees, corresponding to 360 nodes. For each node in the grid, the current value at that node is calculated from the adjacent valid samples using linear interpolation, resulting in a 360-point phase domain signal sequence uniformly distributed in the travel phase domain.

[0030] The quality index is calculated based on two components. The coverage component reflects whether the number of valid samples is sufficient: In this embodiment, the minimum number of samples is preset to 250. The current number of valid samples is divided by 250 and clipped to the range of zero to one. When the number of valid samples reaches or exceeds 250, the coverage component is 1; otherwise, it decreases linearly proportionally. The goodness-of-fit component reflects whether the interpolation result is reliable: The phase domain signal is substituted back at the travel phase position of the valid samples. The root mean square of the deviation between the substituted value and the original valid sample value is calculated, and then divided by the root mean square of the original valid sample value to obtain the normalized interpolation residual. This residual is subtracted from one and clipped to the range of zero to one as the goodness-of-fit component. The smaller the residual, the closer the goodness-of-fit component is to one. Multiplying the coverage component and the goodness-of-fit component yields the quality index, with a value range of zero to one. The quality index serves as a quantitative basis for signal reliability in subsequent steps. When the signal quality is poor, the influence of features on control decisions is automatically reduced to prevent erroneous actions.

[0031] S3. Extract the pre-determined vibration amplitude of each target order from the phase domain signal; generate the order vibration intensity index based on the vibration amplitude, quality index and pre-acquired reference amplitude parameters; The method for obtaining the reference amplitude parameter includes: The system acquires historically collected current signals, performs order analysis on the current signals according to the stroke phase, extracts vibration amplitude samples for each target order, performs statistical analysis on the vibration amplitude samples, and obtains the baseline mean and baseline dispersion for each target order, which are used as baseline amplitude parameters.

[0032] The generation of the order vibration intensity index based on vibration amplitude, quality index, and pre-acquired reference amplitude parameters includes: For each target order, the deviation of the vibration amplitude from the benchmark mean is measured using the benchmark dispersion as a reference scale. The deviation is then clipped to the non-negative range and the upper limit is restricted to obtain the normalized excess for that order. The normalized excess of each target order is weighted and summed according to preset weights to obtain the weighted vibration intensity; The weighted vibration intensity is monotonically fused with the quality index. The smaller the quality index, the smaller the fusion result, thus obtaining the mass-weighted vibration intensity. By imposing a limit on the maximum change in mass-weighted vibration intensity per control cycle, the order vibration intensity index is obtained.

[0033] In this embodiment, the target order is determined through offline analysis before the equipment is put into use. For the harmonic reducer, the flexible wheel and rigid wheel complete two full meshing cycles per revolution of the elliptical contour of the wave generator. Therefore, the most basic meshing excitation occurs at the 2nd order, and its harmonics, the 4th and 6th orders, usually also have significant amplitudes. In actual determination, the joint is operated under conditions covering a typical speed range, and current signals are collected. After processing the current signals according to the stroke phase, order analysis is performed, and the order with an amplitude significantly higher than the background noise is selected as the target order. In this embodiment, three target orders are selected: the 2nd, 4th, and 6th orders.

[0034] The vibration amplitude of the phase domain signal is calculated at the frequency points corresponding to each target order. The phase domain signal has 360 points covering a 180-degree range. The 2nd order corresponds to one complete cycle within the 180-degree range, the 4th order to two cycles, and the 6th order to three cycles. The frequency points corresponding to each target order are the 1st, 2nd, and 3rd frequency components, respectively. A Discrete Fourier Transform is calculated for the phase domain signal, and the magnitude of the transform result at each target frequency point is taken as the vibration amplitude for that order. Only the magnitudes at three target frequency points are calculated, eliminating the need for a complete spectrum analysis across the entire frequency band, resulting in a very small computational load.

[0035] The reference amplitude parameters are collected in advance under normal operating conditions of the joint. Current signals are collected for a sufficient period of time while the joint is running at a representative speed and load. After processing according to the stroke phase, the vibration amplitude of each target order is extracted. The mean and standard deviation of the collected amplitude samples are calculated and stored as the reference mean and reference dispersion for that order, respectively. Taking the second order as an example, if the mean of the second-order amplitude samples collected under normal operating conditions is 0.8 amperes and the standard deviation is 0.15 amperes, then the reference mean is 0.8 amperes and the reference dispersion is 0.15 amperes.

[0036] When generating vibration intensity indices for different orders, the difference between the current vibration amplitude and the baseline mean is calculated for each target order. This difference is divided by the baseline dispersion to obtain the normalized excess, which reflects how many standard deviations the current vibration amplitude exceeds the normal level. A negative difference indicates that the current amplitude is below the normal mean and is clipped to zero. If the difference is too large, the upper limit is set to 3 to prevent occasional outliers from dominating subsequent calculations. Taking order 2 as an example, if the current amplitude is 1.25 amperes, the normalized excess is (1.25 - 0.8) divided by 0.15, resulting in 3, which exactly reaches the upper limit. The normalized excesses for the three target orders are summed with equal weights and then divided by 3 to obtain the weighted vibration intensity, with a value ranging from zero to three.

[0037] The weighted vibration intensity is multiplied by the quality index to obtain the mass-weighted vibration intensity. When the quality index is 1, the fusion result is not compressed; when the quality index is 0.6, the fusion result is compressed to 60% of its original value. When the signal quality is poor, the influence of the feature on the control decision is automatically reduced. A maximum change limit per control cycle is imposed on the mass-weighted vibration intensity. In this embodiment, the maximum change per cycle is set to 0.06 (approximately 2% of the range when the value is between 0 and 3), preventing abrupt changes in the index value between adjacent cycles that could lead to sudden changes in subsequent control parameters. This yields the order vibration intensity index R, whose value ranges from 0 to 3.

[0038] S4. When a reversal in the angular velocity direction is detected, the risk level of the reverse impact is determined based on the change in vibration energy before and after the reversal and the quality indicators. The greater the change in vibration energy, the higher the risk level. The determination of the reverse impact risk level based on the change in vibration energy and quality indicators before and after the overturning includes: Monitor the sign of the angular velocity signal. When the sign of the angular velocity changes and the new direction is maintained for a subsequent preset number of control cycles, confirm a reverse event. After confirming the reverse event, the energy metric values ​​of each target order vibration amplitude are calculated within each preset time window before the direction reversal and after the direction confirmation is completed, so as to obtain the energy before the reversal and the energy after the reversal. The two time windows have the same length. The risk level of the reverse shock is determined based on the degree to which the energy after the reversal exceeds the energy before the reversal. The greater the degree of excess, the higher the risk level. When the degree of excess does not exceed zero, it corresponds to the lowest risk level. When the quality index is not higher than the preset quality threshold, or when the absolute value of the angular velocity is continuously lower than the preset low speed threshold, the reverse impact risk level will be maintained at a higher risk level; when the quality index recovers to be higher than the preset quality threshold and the absolute value of the angular velocity is no longer continuously lower than the preset low speed threshold, the reverse impact risk level will be determined based on the degree of excess.

[0039] In this embodiment, the sign of the angular velocity signal is continuously monitored. When the angular velocity sign changes from positive to negative or from negative to positive, the new direction must be maintained for the next 5 consecutive control cycles before a reverse event is confirmed.

[0040] After confirming the reverse event, the sum of the squares of the vibration amplitudes of each target order within the 50 high-speed state control cycles before the direction reversal is taken as the energy before the reversal. The sum of the squares of the vibration amplitudes of each target order within the 50 high-speed state control cycles after the direction confirmation is taken as the energy after the reversal. Both windows only count valid samples under high-speed conditions and have the same window length. 50 high-speed state control cycles correspond to 50 milliseconds, sufficient to accumulate a large number of vibration amplitude samples, making the energy estimation statistically representative. The energy after the reversal minus the energy before the reversal, divided by the energy before the reversal, yields the relative change. A positive and large relative change indicates a significant enhancement of the meshing excitation after the reversal, resulting in a stronger impact; a value close to zero or negative indicates a smoother reversal process with a weaker impact.

[0041] In this embodiment, the risk level is divided into three levels: low, medium, and high. A relative change not exceeding zero corresponds to a low level; a relative change between zero and 0.5 corresponds to a medium level; and a relative change exceeding 0.5 corresponds to a high level. The threshold of 0.5 is selected based on the distribution of relative changes in several reverse events collected under representative operating conditions, using the natural boundary points in the distribution as a reference and considering the debugging results.

[0042] When the quality index is not higher than 0.6, it indicates that the current signal quality is insufficient to support reliable energy estimation. The risk level is maintained at medium, handling uncertainties in a conservative manner. A pause where the absolute value of the angular velocity is consistently below 0.2 rad / s also remains at a medium level, as a reversal is likely to occur immediately after the pause ends; maintaining a higher level in advance prepares for the upcoming handover. Once the quality index recovers to above 0.6 and the absolute value of the angular velocity no longer consistently falls below 0.2 rad / s, the risk level is reverted to being determined normally based on the relative change.

[0043] S5. Based on the order vibration intensity index and the reverse impact risk level, the torque command is shaped and the shaped torque command is output.

[0044] The process of shaping the torque command based on the order vibration intensity index and the reverse impact risk level includes: Based on the product of the current rotational speed and each target order, the notch center frequency corresponding to each target order is determined. The notch depth is set to a value that increases monotonically and is limited as the vibration intensity index of the order increases. The torque command is subjected to notch filtering with notch center frequency and notch depth as parameters to obtain the notch-filtered torque. The upper limit of the torque change rate is set to a value that decreases monotonically with the increase of the order vibration intensity index and decreases monotonically with the increase of the reverse impact risk level, thereby imposing a torque change rate limit on the torque after the notch wave. When the torque of the current control cycle has the opposite sign to the torque of the previous control cycle and the reverse impact risk level exceeds the preset risk threshold, within the range where the absolute value of the torque is lower than the preset buffer threshold, the upper limit of the torque change rate is replaced with the upper limit of the buffer segment rate, which is smaller than the current upper limit of the torque change rate, and a shaping torque command is output.

[0045] The setting of the notch depth to a value that monotonically increases with the increase of the order vibration intensity index and is limited includes: The notch depth is limited between a preset minimum notch depth and a preset maximum notch depth, and an upper limit is imposed on the change in notch depth per control cycle, so that the notch depth smoothly transitions to the target value.

[0046] In this embodiment, the conventional servo position controller outputs a torque command in each control cycle, and the shaping module processes the command before it is sent to the driver.

[0047] First, determine the notch filter center frequency for each target order based on the current rotational speed. Convert the current rotational speed to revolutions per second (RPM) and multiply by the order to obtain the corresponding frequency. For example, when the current rotational speed is 30 rpm (0.5 RPM), the 2nd order corresponds to 1 Hz, the 4th order to 2 Hz, and the 6th order to 3 Hz. The notch filter center frequency is updated in real time as the rotational speed changes, always aligning with the actual meshing excitation frequency.

[0048] The notch depth is determined by the vibration intensity index R. In this embodiment, R ranges from zero to three, and the notch depth linearly maps to R between 0.05 and 0.35. When R is zero, the notch depth is 0.05, and when R is three, the notch depth is 0.35. The upper limit of the notch depth is set to 0.35 to retain sufficient phase margin and prevent excessively deep notches from damaging system stability. Independent dual second-order IIR notch filters are set for the three target orders, with their center frequencies tracked independently. The frequency points of the three filters do not overlap, and the cascade order does not affect the suppression effect of each order. The notch depth changes by a maximum of 0.001 in each control cycle, allowing for a smooth transition of the notch filter parameters and avoiding abrupt changes in the filter output caused by coefficient jumps. The conventional torque command is processed sequentially through the three notch filters to obtain the notched torque.

[0049] The upper limit of the torque change rate is determined based on the order vibration intensity index R and the reverse impact risk level. Assuming the rated allowable torque change rate of the actuator is S0 (in units of rated torque per millisecond), the base rate upper limit is S0 multiplied by (one minus R divided by six). When R is zero, the base rate upper limit remains at S0; when R is three, the base rate upper limit decreases to 50% of S0. Based on this, no reduction is made for low risk levels, the base rate upper limit is multiplied by 0.7 for medium risk levels, and multiplied by 0.4 for high risk levels, resulting in the final upper limit of the torque change rate. A rate limit is applied to the torque after notch filtering, ensuring that the torque change in each control cycle does not exceed the rate upper limit multiplied by the control cycle length of 1 millisecond.

[0050] The reverse impact risk level is updated after each reverse event is confirmed and the energy comparison before and after the reversal is completed. The updated level is continuously saved for use when the torque sign reverses again. When the shaping torque of the current control cycle is opposite in sign to the shaping torque of the previous control cycle, it indicates that the torque has just passed zero, corresponding to the moment of joint forward / reverse switching. When the same joint reverses continuously under similar working conditions, the impact level of the previous reverse has a strong predictive reference value for the current reverse. Therefore, it is reasonable to use the risk level updated after the last reverse as the basis for the current zero-crossing processing. If the currently saved risk level exceeds the preset risk threshold (in this embodiment, levels 1 and above are triggered), the upper limit of the speed is replaced with the upper limit of the buffer segment speed in the range where the absolute value of the torque is lower than the preset buffer threshold. In this embodiment, the buffer threshold is 10% of the rated torque, and the upper limit of the buffer segment speed is 30% of the current upper limit of the speed, so that the torque changes more slowly near the zero crossing, reducing the impact when the tooth surface re-contacts. After the absolute value of the torque exceeds the buffer threshold, the normal upper limit of the speed is restored. When the risk level is low, buffer processing is not entered, and the torque passes through the zero crossing at the normal speed, without introducing unnecessary response lag.

[0051] After notch filtering, rate limiting, and zero-crossing buffering, the shaping torque command is output to the driver current loop for execution. The periodic excitation component in the shaping torque command that is synchronized with the gear meshing frequency is suppressed by the notch filter, and the torque change rate is limited to a range that matches the current vibration state and shock risk, thus improving the peak value of the joint's following error and the reverse shock.

[0052] Example 2 Please see Figure 2 This invention provides a servo motor control system based on data processing, used to implement a servo motor control method based on data processing, comprising: The stroke phase construction module is used to acquire the angle signal and angular velocity signal of the servo motor, and to perform direction sensing accumulation on the angle signal based on the direction of the angular velocity signal to obtain the stroke phase and an effective marker to distinguish the accumulation state from the stop accumulation state. The phase domain rearrangement module is used to acquire the current signal of the servo motor, filter the valid samples corresponding to the valid markers from the current signal, rearrange the valid samples at equal intervals according to the stroke phase to obtain the phase domain signal, and calculate the quality index based on the sufficiency of the number of valid samples and the goodness of fit of the rearrangement. The vibration intensity analysis module is used to extract the vibration amplitude of each target order from the phase domain signal; and to generate the order vibration intensity index based on the vibration amplitude, quality index and pre-acquired reference amplitude parameters. The impact risk assessment module is used to determine the reverse impact risk level based on the change in vibration energy before and after the rollover and the quality index when a rollover in the angular velocity direction is detected. The greater the change in vibration energy, the higher the risk level. The torque command shaping module is used to shape the torque command based on the order vibration intensity index and the reverse impact risk level, and output the shaped torque command.

[0053] In the several embodiments provided by this invention, it should be understood that the disclosed systems, apparatuses, and methods can be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative; for instance, the division of units is only one method, and in actual implementation, there may be other division methods. For example, multiple units or components may be combined or integrated into another system, or some features may be ignored or not executed. Furthermore, the coupling or direct coupling or communication connection shown or discussed may be through some interfaces; the indirect coupling or communication connection between apparatuses or units may be electrical, mechanical, or other forms.

[0054] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in the present invention should be included within the scope of protection of the present invention.

[0055] In conclusion, the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A servo motor control method based on data processing, characterized in that, include: Acquire the angle and angular velocity signals of the servo motor, perform directional sensing accumulation on the angle signal based on the direction of the angular velocity signal, and obtain the stroke phase and an effective marker to distinguish the accumulation state from the stop accumulation state. The current signal of the servo motor is acquired, and valid samples corresponding to valid markers are selected from the current signal. The valid samples are rearranged at equal intervals according to the stroke phase to obtain the phase domain signal. The quality index is calculated based on the sufficiency of the number of valid samples and the goodness of fit of the rearrangement. Extract the pre-determined vibration amplitude of each target order from the phase domain signal; generate the order vibration intensity index based on the vibration amplitude, quality index and pre-acquired reference amplitude parameters; When a reversal in the angular velocity direction is detected, the risk level of the reverse impact is determined based on the change in vibration energy before and after the reversal and the quality indicators. The greater the change in vibration energy, the higher the risk level. Based on the order vibration intensity index and the reverse impact risk level, the torque command is shaped and the shaped torque command is output.

2. The servo motor control method based on data processing according to claim 1, characterized in that, The direction sensing and accumulation of the angle signal based on the direction of the angular velocity signal includes: Each control cycle calculates the angle increment of the angle signal at adjacent sampling times; Hysteresis judgment with upper and lower switching thresholds is applied to the angular velocity signal, where the upper switching threshold is greater than the lower switching threshold; Hysteresis detection includes: when the absolute value of angular velocity exceeds the upper switching threshold and remains above the upper switching threshold for multiple consecutive control cycles, it is confirmed that the current state is high speed; when the absolute value of angular velocity is lower than the lower switching threshold, it is confirmed that the current state is low speed; when the absolute value of angular velocity is between the upper and lower switching thresholds, the current state remains unchanged. In each control cycle at high speed, the absolute value of the angle increment of this control cycle is accumulated into the stroke phase, and this control cycle is marked as valid. In each control cycle at low speed, the stroke phase remains unchanged, and the current control cycle is marked as invalid.

3. The servo motor control method based on data processing according to claim 1, characterized in that, The process of rearranging the effective samples at equal intervals according to their travel phase to obtain a phase domain signal, and calculating the quality index based on the sufficiency of the number of effective samples and the goodness of fit of the rearrangement, includes: With the goal of an equally spaced phase grid covering the effective sample travel phase range, the effective samples are mapped to each node of the phase grid through interpolation to obtain a phase domain signal that is uniformly distributed in the travel phase domain. The coverage rate component is obtained based on the sufficiency of the number of effective samples relative to the preset minimum number of samples. The phase domain signal is substituted back at the travel phase position of the effective samples, and the goodness of fit component is obtained based on the normalized fitting error between the substituted value and the original effective sample value. The smaller the normalized fitting error, the larger the goodness of fit component. The coverage component and the goodness-of-fit component are combined to obtain the quality index. The larger the coverage component and the goodness-of-fit component, the larger the quality index.

4. The servo motor control method based on data processing according to claim 1, characterized in that, The method for obtaining the reference amplitude parameter includes: The system acquires historically collected current signals, performs order analysis on the current signals according to the stroke phase, extracts vibration amplitude samples for each target order, performs statistical analysis on the vibration amplitude samples, and obtains the baseline mean and baseline dispersion for each target order, which are used as baseline amplitude parameters.

5. The servo motor control method based on data processing according to claim 4, characterized in that, The generation of the order vibration intensity index based on vibration amplitude, quality index, and pre-acquired reference amplitude parameters includes: For each target order, the deviation of the vibration amplitude from the benchmark mean is measured using the benchmark dispersion as a reference scale. The deviation is then clipped to the non-negative range and the upper limit is restricted to obtain the normalized excess for that order. The normalized excess of each target order is weighted and summed according to preset weights to obtain the weighted vibration intensity; The weighted vibration intensity is monotonically fused with the quality index. The smaller the quality index, the smaller the fusion result, thus obtaining the mass-weighted vibration intensity. By imposing a limit on the maximum change in mass-weighted vibration intensity per control cycle, the order vibration intensity index is obtained.

6. The servo motor control method based on data processing according to claim 1, characterized in that, The determination of the reverse impact risk level based on the change in vibration energy and quality indicators before and after the overturning includes: Monitor the sign of the angular velocity signal. When the sign of the angular velocity changes and the new direction is maintained for a subsequent preset number of control cycles, confirm a reverse event. After confirming the reverse event, the energy metric values ​​of each target order vibration amplitude are calculated within each preset time window before the direction reversal and after the direction confirmation is completed, so as to obtain the energy before the reversal and the energy after the reversal. The two time windows have the same length. The risk level of the reverse shock is determined based on the degree to which the energy after the reversal exceeds the energy before the reversal. The greater the degree of excess, the higher the risk level. When the degree of excess does not exceed zero, it corresponds to the lowest risk level. When the quality index is not higher than the preset quality threshold, or when the absolute value of the angular velocity is continuously lower than the preset low speed threshold, the reverse impact risk level will be maintained at a higher risk level; when the quality index recovers to be higher than the preset quality threshold and the absolute value of the angular velocity is no longer continuously lower than the preset low speed threshold, the reverse impact risk level will be determined based on the degree of excess.

7. The servo motor control method based on data processing according to claim 1, characterized in that, The process of shaping the torque command based on the order vibration intensity index and the reverse impact risk level includes: Based on the product of the current rotational speed and each target order, the notch center frequency corresponding to each target order is determined. The notch depth is set to a value that increases monotonically and is limited as the vibration intensity index of the order increases. The torque command is subjected to notch filtering with notch center frequency and notch depth as parameters to obtain the notch-filtered torque. The upper limit of the torque change rate is set to a value that decreases monotonically with the increase of the order vibration intensity index and decreases monotonically with the increase of the reverse impact risk level, thereby imposing a torque change rate limit on the torque after the notch wave. When the torque of the current control cycle has the opposite sign to the torque of the previous control cycle and the reverse impact risk level exceeds the preset risk threshold, within the range where the absolute value of the torque is lower than the preset buffer threshold, the upper limit of the torque change rate is replaced with the upper limit of the buffer segment rate, which is smaller than the current upper limit of the torque change rate, and a shaping torque command is output.

8. The servo motor control method based on data processing according to claim 7, characterized in that, The setting of the notch depth to a value that monotonically increases with the increase of the order vibration intensity index and is limited includes: The notch depth is limited between a preset minimum notch depth and a preset maximum notch depth, and an upper limit is imposed on the change in notch depth per control cycle, so that the notch depth smoothly transitions to the target value.

9. A servo motor control system based on data processing, used to implement the servo motor control method based on data processing as described in any one of claims 1-8, characterized in that, include: The stroke phase construction module is used to acquire the angle signal and angular velocity signal of the servo motor, and to perform direction sensing accumulation on the angle signal based on the direction of the angular velocity signal to obtain the stroke phase and an effective marker to distinguish the accumulation state from the stop accumulation state. The phase domain rearrangement module is used to acquire the current signal of the servo motor, filter the valid samples corresponding to the valid markers from the current signal, rearrange the valid samples at equal intervals according to the stroke phase to obtain the phase domain signal, and calculate the quality index based on the sufficiency of the number of valid samples and the goodness of fit of the rearrangement. The vibration intensity analysis module is used to extract the vibration amplitude of each target order from the phase domain signal; and to generate the order vibration intensity index based on the vibration amplitude, quality index and pre-acquired reference amplitude parameters. The impact risk assessment module is used to determine the reverse impact risk level based on the change in vibration energy before and after the rollover and the quality index when a rollover in the angular velocity direction is detected. The greater the change in vibration energy, the higher the risk level. The torque command shaping module is used to shape the torque command based on the order vibration intensity index and the reverse impact risk level, and output the shaped torque command.

Images