A dynamic stabilization drive control method and apparatus for a gyroscope

Abstract

This invention relates to the field of control system technology, specifically to a dynamic stabilization drive control method and apparatus for a gyroscope. The method includes: acquiring the drive state signal and operating temperature signal of the gyroscope; determining the dominant interference axis and the main interference reference signal of a triaxial accelerometer; performing power spectrum analysis on the drive state signal to obtain the real-time resonant frequency; determining the degree of temperature drift based on the operating temperature signal; determining the instantaneous frequency of the main interference reference signal; determining the distance difference between the real-time resonant frequency and the instantaneous frequency of the main interference; combining the distance difference, the degree of temperature drift, and the instantaneous frequency of the main interference to determine the target notch parameters and target filter of the filter; converting the drive state signal into a purified drive signal through the target filter; and controlling the gyroscope drive based on the purified drive signal. The technical solution of this invention improves the stability and rapid adjustment capability of the gyroscope drive mode, as well as the corresponding measurement accuracy.

Description

Technical Field

[0001] This invention relates to the field of control system technology, and specifically to a dynamic stabilization drive control method and device for a gyroscope. Background Technology

[0002] With the rapid development of microelectromechanical systems (MEMS) technology, gyroscopes, due to their advantages of small size, low cost, and high reliability, have been widely used in consumer electronics, automotive industry, aerospace, and precision navigation. The core function of a gyroscope is to accurately measure the angular velocity of a moving object. Its working principle is mainly based on the Coriolis effect: driving a mass block to perform high-frequency, stable resonant motion in a plane, when the object rotates, the mass block will be subjected to a Coriolis force perpendicular to both the driving direction and the rotation direction. The displacement in another dimension caused by this force is proportional to the input angular velocity, thus achieving angular velocity sensing.

[0003] When a gyroscope operates under complex conditions, the stable vibration of its drive shaft is easily affected by both external multi-directional broadband mechanical vibration and internal temperature drift, leading to drive frequency shift and amplitude fluctuation, which in turn affects the measurement accuracy and reliability of the entire instrument. Traditional drive control methods are difficult to distinguish and effectively suppress vibration interference from the direction of strongest energy in real time, and lack the ability to adapt to time-varying resonance parameters caused by temperature changes. They often use filters with fixed parameters or a single control loop, resulting in incomplete interference suppression or damage to useful signals, making it difficult to balance the stability and efficiency of the drive mode. Summary of the Invention

[0004] To address the technical problems of existing gyroscope drive control methods, such as incomplete interference suppression or damage to useful signals, and the difficulty in balancing the stability and efficiency of the drive mode, resulting in low measurement accuracy, the present invention aims to provide a dynamic stable drive control method and device for gyroscopes. The specific technical solution adopted is as follows:

[0005] This invention provides a dynamic stabilization drive control method for a gyroscope, the method comprising:

[0006] The drive status signal and operating temperature signal of the gyroscope are acquired, and the dominant interference axis is determined based on the triaxial acceleration signal of the triaxial accelerometer and the main interference reference signal is obtained.

[0007] The real-time resonant frequency is obtained by power spectrum analysis of the drive state signal, the degree of temperature drift is determined based on the operating temperature signal, and the instantaneous frequency of the main interference reference signal relative to the nominal drive frequency of the gyroscope is determined.

[0008] The distance difference between the real-time resonant frequency and the instantaneous frequency of the main interference is determined, and the target notch parameters of the filter are determined by combining the degree of temperature drift, the instantaneous frequency of the main interference, and the distance difference.

[0009] The target filter is determined using the target notch parameters. The drive state signal is converted into a purified drive signal through the target filter. The gyroscope drive is then controlled based on the purified drive signal.

[0010] Furthermore, the determination of the dominant interference axis and the acquisition of the main interference reference signal from the triaxial acceleration signal based on the triaxial accelerometer includes:

[0011] Acquire the mean square values ​​of the triaxial acceleration signals from the triaxial accelerometer within a preset time window;

[0012] The axis corresponding to the maximum mean square value is taken as the dominant disturbance axis among the three axes, and the corresponding acceleration signal is taken as the main disturbance reference signal.

[0013] Furthermore, the step of performing power spectrum analysis on the driving state signal to obtain the real-time resonant frequency includes:

[0014] The original drive state signal is input into a zero-phase FIR bandpass filter to obtain a filtered drive signal;

[0015] Power spectrum analysis is performed on multiple consecutive filtered drive signal segments to obtain various power spectrum curves.

[0016] The driving peak frequencies in each power spectrum curve are obtained based on the spectral peak characteristics that distinguish the driving signal from the interference signal.

[0017] The real-time resonant frequency is obtained by performing a moving average filter on the peak frequencies of each driving spectrum.

[0018] Furthermore, determining the degree of temperature drift based on the operating temperature signal includes:

[0019] Determine the operating temperature sequence of the operating temperature signal within the most recent time window, and determine the correlation coefficient and variation relationship between the operating temperature and the resonant frequency in the operating temperature sequence;

[0020] Determine the current average rate of change of the operating temperature over time at the current moment, and use the correlation coefficient, the relationship of change, and the current average rate to determine the degree of current temperature drift.

[0021] Further, determining the instantaneous frequency of the main interference reference signal relative to the nominal drive frequency of the gyroscope includes:

[0022] The corresponding linear phase FIR filter is determined based on the gyroscope's nominal drive frequency and the preset maximum expected frequency deviation.

[0023] The main interference reference signal is input into the linear phase FIR filter to obtain the filtered main interference reference signal;

[0024] The instantaneous frequency of the main interference is obtained by determining the instantaneous frequency offset of the main interference reference signal after demodulation and filtering using the nominal driving frequency of the gyroscope as the reference frequency and the instantaneous phase offset of the main interference reference signal relative to the nominal driving frequency of the gyroscope.

[0025] Furthermore, determining the distance difference between the real-time resonant frequency and the instantaneous frequency of the main interference includes:

[0026] Determine the power spectrum curve corresponding to the real-time resonant frequency, and based on the power spectrum curve, determine the frequency range width corresponding to when the peak amplitude of the resonant peak drops to half the power bandwidth;

[0027] The absolute value of the difference between the real-time resonant frequency and the instantaneous frequency of the main interference is determined, and the distance difference is determined by the ratio between the absolute value of the difference and the width of the frequency range.

[0028] Furthermore, determining the target notch parameters of the filter by combining the degree of temperature drift, the instantaneous frequency of the main interference, and the distance difference includes:

[0029] A distance-dependent protection factor is constructed using the aforementioned distance difference, which is negatively correlated with the notch filter bandwidth design range.

[0030] The interference fluctuation level of the main interference instantaneous frequency is determined by using the main interference instantaneous frequency sequence, and the main interference instantaneous frequency is used as the target notch filter center frequency of the filter.

[0031] The target notch filter bandwidth is obtained by combining the degree of temperature drift, the degree of interference fluctuation, and the distance-related protection factor.

[0032] Furthermore, the step of determining the target filter using the target notch parameters and converting the drive state signal into a purified drive signal using the target filter includes:

[0033] The digital center angular frequency is obtained using the center frequency of the target notch filter, and the pole radius factor is obtained using the bandwidth of the target notch filter.

[0034] The difference equation of the second-order IIR notch filter is obtained based on the digital center angular frequency and the pole radius factor to determine the corresponding target filter.

[0035] The filtered drive state signal is input into the target filter to convert the output into a purified drive signal.

[0036] Furthermore, the gyroscope drive control based on the purification drive signal includes:

[0037] Determine the quality factor corresponding to the real-time resonant frequency, and use the filtered and smoothed quality factor to obtain the feedforward control quantity;

[0038] Extract the real-time vibration amplitude from the purification drive signal and determine the amplitude error between the real-time vibration amplitude and the expected amplitude.

[0039] The feedback control quantity is obtained by using the amplitude error, and the target driving voltage is obtained by combining the feedforward control quantity and the feedback control quantity to drive the gyroscope.

[0040] The present invention also provides a dynamic stabilization drive control device for a gyroscope, the device being used to implement the dynamic stabilization drive control method for a gyroscope as described in any of the preceding claims; the device comprising:

[0041] The signal acquisition module is used to acquire the drive status signal and operating temperature signal of the gyroscope, and to determine the dominant interference axis and obtain the main interference reference signal based on the triaxial acceleration signal of the triaxial accelerometer.

[0042] The signal analysis module is used to perform power spectrum analysis on the drive state signal to obtain the real-time resonant frequency, determine the degree of temperature drift based on the operating temperature signal, determine the instantaneous frequency of the main interference relative to the nominal drive frequency of the gyroscope, determine the distance difference between the real-time resonant frequency and the instantaneous frequency of the main interference, and determine the target notch parameters of the filter by combining the degree of temperature drift, the instantaneous frequency of the main interference and the distance difference.

[0043] The drive parameter tuning module is used to determine the target filter using the target notch parameters, convert the drive state signal into a purified drive signal through the target filter, and control the gyroscope drive based on the purified drive signal.

[0044] The present invention has the following beneficial effects:

[0045] This invention achieves precise location and frequency tracking of the primary vibration interference source by synchronously acquiring multi-source sensor data and selecting the axis with the highest energy among the three axes of the accelerometer as the main interference analysis axis. By integrating the three physical constraints of interference frequency fluctuation, temperature drift trend, and the relative distance between the interference frequency and the system resonant frequency, the optimal notch filter parameters are dynamically calculated. This allows the filter to adaptively cope with both the time-varying characteristics of vibration interference and the temperature drift of the gyroscope system (or inertial measurement system), ensuring effective suppression of interference while maximizing the protection of the useful drive signal. Finally, the purified drive signal is used for closed-loop control of the system, and feedforward compensation is performed in conjunction with the real-time estimated quality factor. This enables the system to actively and quickly offset the influence of damping changes on the drive force, thereby significantly improving the stability and rapid adaptive adjustment capability of the gyroscope drive mode, enhancing the gyroscope's accuracy in measuring angular velocity, and achieving high stability and robustness of the gyroscope drive mode in complex dynamic environments. Attached Figure Description

[0046] To more clearly illustrate the technical solutions and advantages in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0047] Figure 1 A flowchart illustrating the steps of a dynamic stabilization drive control method for a gyroscope, provided in one embodiment of the present invention.

[0048] Figure 2 This is a detailed flowchart of step S2 in a dynamic stabilization drive control method for a gyroscope provided in an embodiment of the present invention.

[0049] Figure 3 A detailed flowchart of step S2 in a dynamic stabilization drive control method for a gyroscope provided in another embodiment of the present invention;

[0050] Figure 4 This is a detailed flowchart of step S3 in a dynamic stabilization drive control method for a gyroscope provided in an embodiment of the present invention.

[0051] Figure 5 A detailed flowchart of step S3 in a dynamic stabilization drive control method for a gyroscope provided in another embodiment of the present invention;

[0052] Figure 6 This is a schematic diagram of the hardware operating environment of the dynamic stabilization drive control device for a gyroscope involved in the embodiments of the present invention;

[0053] Figure 7 This is a schematic diagram of the frame structure of a dynamic stabilization drive control device for a gyroscope according to an embodiment of the present invention. Detailed Implementation

[0054] To further illustrate the technical means and effects adopted by the present invention to achieve its intended purpose, the following, in conjunction with the accompanying drawings and preferred embodiments, details the specific implementation, structure, features, and effects of a dynamic stabilization drive control method for a gyroscope proposed according to the present invention. In the following description, different "one embodiment" or "another embodiment" do not necessarily refer to the same embodiment. Furthermore, specific features, structures, or characteristics in one or more embodiments can be combined in any suitable form.

[0055] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains.

[0056] The following description, in conjunction with the accompanying drawings, details a specific scheme for a dynamic stabilization drive control method for a gyroscope provided by the present invention.

[0057] Example 1:

[0058] For a dynamic stabilization drive control method for a gyroscope provided by this invention, please refer to [link to relevant documentation]. Figure 1 The diagram illustrates a flowchart of a dynamic stabilization drive control method for a gyroscope according to an embodiment of the present invention.

[0059] The dynamic stabilization drive control method for a gyroscope includes:

[0060] Step S1: Obtain the drive status signal and operating temperature signal of the gyroscope, determine the dominant interference axis based on the triaxial acceleration signal of the triaxial accelerometer, and obtain the main interference reference signal.

[0061] In this embodiment, during the operation of the gyroscope, the vibration state of its drive shaft is easily affected by various factors such as environmental vibration and temperature drift, and the change in its own motion state will also affect the detection accuracy through mechanical coupling.

[0062] High-precision synchronous data acquisition is adopted, and multiple core signals of the inertial measurement system are synchronously acquired at a fixed sampling frequency fs (100Hz), including: the drive status signal and operating temperature signal of the gyroscope and the triaxial acceleration signal of the triaxial accelerometer.

[0063] For the drive state signal: the demodulated signal output by the detection electrode of the gyroscope drive axis is collected. This signal contains effective information of the drive vibration displacement envelope, and is superimposed with interference components such as environmental vibration and circuit noise. It is the core signal reflecting the drive state of the gyroscope, and is denoted as gyroscope signal or drive state signal S(t).

[0064] For the operating temperature signal: The temperature value output by the temperature sensor can be acquired by using a patch-type platinum resistance temperature sensor, which is placed close to the sensitive (e.g., easily reflects temperature changes) structural area of ​​the gyroscope, and the operating temperature signal T(t) of the gyroscope is acquired in real time.

[0065] For triaxial acceleration signals: the raw triaxial acceleration signals output by the triaxial accelerometer are acquired to directly sense the three-dimensional vibration state of the carrier, which includes three mutually perpendicular axial acceleration signals; it should be noted that the mechanical coupling coefficients of each axis to the gyroscope driving mode are on the same order of magnitude.

[0066] Analysis shows that the vibration energy is unevenly distributed along different axes, and the strongest direction of interference has the greatest impact on the gyroscope. The greater the vibration energy along a certain axis of the three axes, the greater the coupling energy transmitted to the sensitive structure of the gyroscope in that direction, and the stronger the potential damage to the drive ring. Therefore, it is necessary to select the axis with the largest energy as the main interference analysis axis, that is, the dominant interference axis, and obtain its corresponding main interference reference signal.

[0067] Specifically, step S1, which involves determining the dominant interference axis and obtaining the main interference reference signal based on the triaxial acceleration signal from the triaxial accelerometer, includes:

[0068] Acquire the mean square values ​​of the triaxial acceleration signals from the triaxial accelerometer within a preset time window;

[0069] The axis corresponding to the maximum mean square value is taken as the dominant disturbance axis among the three axes, and the corresponding acceleration signal is taken as the main disturbance reference signal.

[0070] In this embodiment, a sliding time window (preset time window, which can be adjusted) with a duration of 10 driving cycles closest to the current time is set. Within the corresponding time window, the mean square values ​​of the three-axis acceleration signals are obtained respectively, which are then used as estimates of the instantaneous vibration energy in the corresponding direction of each axis.

[0071] The axis with the largest energy estimate is selected as the dominant interference axis. This axis carries the largest input mechanical vibration energy at the current moment, and therefore the interference transmitted and coupled into the gyroscope drive mode along this axis is the most significant. The corresponding original acceleration signal is then selected as the main interference reference signal. If multiple axes have the same mean square value, the dominant interference axis is determined according to a preset priority (e.g., X>Y>Z), or the dominant interference axis from the previous moment is kept unchanged.

[0072] Step S2: Perform power spectrum analysis on the drive state signal to obtain the real-time resonant frequency, determine the degree of temperature drift based on the operating temperature signal, and determine the instantaneous frequency of the main interference reference signal relative to the nominal drive frequency of the gyroscope.

[0073] Specifically, please refer to Figure 2 In one embodiment, step S2, which involves performing power spectrum analysis on the driving state signal to obtain the real-time resonant frequency, includes:

[0074] Step S21: Input the original drive state signal into a zero-phase FIR bandpass filter to obtain a filtered drive signal;

[0075] Step S22: Perform power spectrum analysis on multiple consecutive filtered drive signal segments to obtain various power spectrum curves;

[0076] Step S23: Obtain the driving peak frequency in each power spectrum curve based on the spectral peak characteristics that distinguish the driving signal from the interference signal;

[0077] Step S24: Perform a moving average filter on the peak frequencies of each driving spectrum to obtain the real-time resonant frequency.

[0078] In this embodiment, the core signal S(t) of the gyroscope driving state (original) is fed into a zero-phase FIR (Finite Impulse Response) bandpass filter to obtain the filtered gyroscope signal (filtered driving signal) S'(t); S'(t) is the electrical reflection of the actual motion response of the sensitive structure inside the gyroscope, and is limited to the components within a finite bandwidth near the nominal driving frequency of the gyroscope.

[0079] In real-world high-dynamic application scenarios, gyroscopes are constantly subjected to complex and ever-changing environmental excitations and internal heat dissipation. The resonant frequency and damping of their mechanical structure change with time and environment, causing mismatch in control systems based on fixed or nominal models. Specifically, the phase-locked loop may deviate from the true resonant point, and the automatic gain control cannot accurately compensate for the changing damping, resulting in instability of the drive mode.

[0080] According to prior knowledge, the power spectrum shape of a linear second-order resonant system, i.e., a gyroscope-driven mode, is always uniquely determined by its current intrinsic frequency and damping, regardless of how its parameters drift. The core objective of the gyroscope drive loop is to maintain vibration. Therefore, in the power spectrum of the drive signal, the response component excited by the driving force and located at the current true resonant frequency has a much larger amplitude than any random vibration disturbance component in the same frequency band.

[0081] Power spectrum analysis was performed on the filtered gyroscope signal S'(t) extracted above. The power spectral density was calculated using the improved Welch method. The Hanning window was selected as the window function, with a window length of ≥4096 (empirical value) sampling points (corresponding to one frame, i.e. a segment of time-domain signal). Zero-padding was used to further increase the spectral density. The overlap rate was 50% to balance the frequency resolution and spectral estimation variance, thereby obtaining the power spectrum curve corresponding to the gyroscope signal S'(t). The power spectrum curves corresponding to multiple consecutive filtered drive signal segments (based on frame segmentation) were obtained.

[0082] Taking the power spectrum curve of one segment as an example, several candidate frequencies are initially obtained by searching for the dominant peak frequency in the power spectrum;

[0083] Since the filtered gyroscope signal S'(t) originally contains strong driving signals and interference signals, even after adaptive notch filtering, there may still be residual interference spectrum peaks. Therefore, it is still necessary to screen the above candidate frequencies.

[0084] Based on the closed-loop control characteristics of the gyroscope, the amplitude of the drive signal is much larger than the amplitude of the random disturbance, and the corresponding amplitude of the drive spectrum peak is also significantly higher than the disturbance spectrum peak.

[0085] Therefore, the peak frequency with the largest amplitude in the power spectrum that is located within the theoretical frequency range of the gyroscope's driving mode (known from the gyroscope's mechanical design) is selected as the driving peak frequency. A moving average filter is used to smooth the driving peak frequencies of multiple consecutive frames. For example, the moving window length is set to 10 frames, and the average value of the driving peak frequencies of multiple frames is obtained as the real-time resonant frequency estimate f. res (t).

[0086] Specifically, please refer to Figure 3 In another embodiment, step S2, determining the degree of temperature drift based on the operating temperature signal, includes:

[0087] Step S201: Determine the working temperature sequence of the working temperature signal within the most recent time window, and determine the correlation coefficient and variation relationship between the working temperature and the resonant frequency in the working temperature sequence;

[0088] Step S202: Determine the current average rate of change of the working temperature over time at the current moment, and determine the degree of current temperature drift using the correlation coefficient, the relationship of change, and the current average rate.

[0089] In this embodiment, the system resonant frequency itself is uncertain, mainly caused by temperature drift. Temperature changes will cause the resonant frequency to drift by changing the elastic modulus of the gyroscope structural material.

[0090] Based on the gyroscope operating temperature signal, obtain the operating temperature sequence corresponding to the gyroscope operating temperature signal within the system's most recent working time, such as within the past 5 hours (the most recent time window, which can be adjusted).

[0091] Based on the above operating temperature sequence, the absolute value of the Pearson correlation coefficient between the (operating) temperature and the resonant frequency is calculated and denoted as the confidence weight S. This value represents the confidence level of the mutual influence between the actual temperature and the resonant frequency. The closer the value is to 1, the more obvious the influence relationship between the two. The confidence weight S is used as a weight to weight the physical drift model. The stronger the correlation, the greater the weight of the predicted value.

[0092] Simultaneously, the ratio between the resonant frequency range and the temperature range within the corresponding 5 hours is calculated to reflect the relationship between the actual temperature and the resonant frequency; the product of this ratio and the aforementioned confidence weight is calculated; thus, the temperature-frequency change ratio is obtained, which reflects the average drift of the resonant frequency caused by each temperature change near the current operating point.

[0093] It should be noted that, to prevent the temperature range in the denominator from being zero during the calculation of the above ratios, a very small regularization parameter, such as 10, can be added to the denominator. -6 ;

[0094] You can also set a short-term historical data window with a size of 100 sampling points, calculate the slope (absolute value) of the linear fit between the current time t and the temperature within the window constructed from the previous sampling times, and use this as the current average rate of temperature change over time.

[0095] Obtain the product between the current average rate and the temperature sampling frequency (which can be the fixed sampling frequency fs mentioned above). This product gives the intensity of the current temperature change trend. This value reflects how fast the current temperature changes. The larger the value, the faster the change.

[0096] This yields the product of the temperature-frequency change ratio and the intensity of the temperature change trend at the sampling time, multiplied by a prediction time window (e.g., 1 second). The absolute value of the result is then obtained to determine the temperature drift term, representing the current degree of temperature drift. f T (t); This value represents the maximum possible drift of the resonant frequency per unit time under the current temperature change trend.

[0097] Specifically, step S2, determining the instantaneous frequency of the main interference reference signal relative to the nominal drive frequency of the gyroscope, includes:

[0098] The corresponding linear phase FIR filter is determined based on the gyroscope's nominal drive frequency and the preset maximum expected frequency deviation.

[0099] The main interference reference signal is input into the linear phase FIR filter to obtain the filtered main interference reference signal;

[0100] The instantaneous frequency of the main interference is obtained by determining the instantaneous frequency offset of the main interference reference signal after demodulation and filtering using the nominal driving frequency of the gyroscope as the reference frequency and the instantaneous phase offset of the main interference reference signal relative to the nominal driving frequency of the gyroscope.

[0101] In some embodiments of the present invention, after determining its instantaneous phase, the instantaneous phase sequence can also be subjected to phase unwrapping to eliminate the discontinuity of phase winding.

[0102] In this embodiment, the maximum expected frequency deviation is set to 200Hz based on experience, and the nominal drive frequency f0 of the gyroscope (an inherent parameter of the gyroscope) is obtained at the same time.

[0103] Therefore, a linear-phase FIR filter with a passband of [f0-200Hz, f0+200Hz] is determined and the group delay is compensated. The main interference reference signal is passed through this filter, and the nominal driving frequency f0 of the gyroscope is used as the reference frequency. The main interference reference signal after passing through the filter is orthogonally regulated, and after low-pass filtering, the baseband complex signal is obtained. By calculating the instantaneous phase of the baseband complex signal, and obtaining its instantaneous phase deviation relative to the nominal driving frequency f0 of the gyroscope through phase difference, the instantaneous frequency fd(t) of the main interference is obtained. This frequency fd(t) represents the frequency value of the external periodic vibration that is coupled to the gyroscope chip body through the axis with the largest energy (the dominant interference axis) at the current moment, and whose frequency is near the gyroscope's operating frequency band.

[0104] Step S3: Determine the distance difference between the real-time resonant frequency and the instantaneous frequency of the main interference, and determine the target notch parameters of the filter by combining the degree of temperature drift, the instantaneous frequency of the main interference, and the distance difference;

[0105] Specifically, please refer to Figure 4 In one embodiment, step S3, determining the distance difference between the real-time resonant frequency and the instantaneous frequency of the main interference, includes:

[0106] Step S31: Determine the power spectrum curve corresponding to the real-time resonant frequency, and determine the frequency range width corresponding to the peak amplitude of the resonant peak dropping to half the power bandwidth based on the power spectrum curve.

[0107] Step S32: Determine the absolute value of the difference between the real-time resonant frequency and the instantaneous frequency of the main interference, and use the ratio between the absolute value of the difference and the width of the frequency range to determine the distance difference.

[0108] In this embodiment, analysis shows that when the interference frequency is close to the system resonant frequency, the notch filter (a type of filter) should be as narrow as possible to protect the useful signal to the greatest extent; when the interference frequency is far from the resonant frequency, the bandwidth of the notch filter can be appropriately widened to improve the stability of interference suppression.

[0109] The power spectrum curve fitted by the above embodiments, that is, the power spectrum curve corresponding to the real-time resonant frequency, first determines the peak amplitude of its resonant peak, calculates the amplitude threshold corresponding to the -3dB bandwidth (here "-" indicates a decrease or attenuation, that is, a decrease to half power, also called half power bandwidth), searches for two frequency points on the left and right sides of the fitted curve that are equal to the amplitude threshold, and the difference between the two is the -3dB bandwidth. Thus, the -3dB bandwidth of the resonant peak is obtained, that is, the frequency range width corresponding to the power spectrum amplitude decreasing to half of its power (existing concept).

[0110] It should be noted that if multiple intersection points appear during the search process, the distance to the estimated real-time resonant frequency f should be selected. res (t) Calculate the bandwidth of the nearest pair of intersections.

[0111] Obtain the real-time resonant frequency estimate f res The absolute value of the difference between the interference frequency (t) and the instantaneous frequency fd(t) of the main interference is used. Based on the -3dB bandwidth of the obtained resonance peak, the ratio between the two is calculated, and then normalized and denoted as the absolute distance (distance difference) d(t) between the interference frequency and the resonance frequency. The larger this value is, the farther the interference frequency is from the system resonance point, and the smaller the threat of the interference. The bandwidth of the notch filter used to suppress the interference can be designed to be relatively wider (to improve the suppression robustness) without worrying about damaging the useful signal. Conversely, the smaller the value is, the greater the threat, and the narrower the bandwidth of the notch filter must be. At this time, the requirement for the interference frequency tracking accuracy is also higher.

[0112] Specifically, please refer to Figure 5 In another embodiment, step S3, which combines the degree of temperature drift, the instantaneous frequency of the main interference, and the distance difference to determine the target notch parameters of the filter, includes:

[0113] Step S301: Use the instantaneous frequency of the main interference as the target notch filter center frequency of the filter, and use the instantaneous frequency sequence of the main interference to determine the interference fluctuation degree of the instantaneous frequency of the main interference.

[0114] Step S302: Construct a distance-related protection factor that is negatively correlated with the notch filter bandwidth design range using the distance difference;

[0115] Step S303: Combine the degree of temperature drift, the degree of interference fluctuation, and the distance-related protection factor to obtain the target notch filter bandwidth of the filter.

[0116] In this embodiment, in practical engineering applications, the notch filter needs to strike a balance between suppressing interference and protecting useful signals. If the notch bandwidth is too wide, it will excessively attenuate the useful driving mode signal, reducing the driving efficiency and measurement accuracy of the gyroscope; if the bandwidth is too narrow, it cannot completely cover the time-varying interference frequency range, resulting in incomplete interference suppression.

[0117] Analysis shows that the core function of a notch filter is to attenuate interference signals at a specific frequency; therefore, the center frequency of the notch filter must be precisely matched with the interference frequency.

[0118] Considering that the instantaneous frequency fd(t) of the main interference can characterize the frequency characteristics of the interference in real time, the center frequency of the notch filter can be directly set as fd(t), which is the target notch filter center frequency of the filter.

[0119] It should be noted that before setting the notch filter parameters, the instantaneous frequency fd(t) of the main interference and the resonant frequency f can also be calculated. res The absolute value of the difference (t). If this absolute value is less than the preset safety protection threshold D, which is set to D=5Hz here, it is determined to be a high-risk area for co-channel interference. At this time, the notch filter bypass strategy, i.e., the frequency locking strategy, is executed to forcibly lock the center frequency of the notch filter at f. res (t)+D or f res At (t)-D, the notch filter bandwidth is set to the preset minimum base bandwidth (e.g., 1Hz) so that the notch filter frequency band avoids the gyroscope drive signal frequency band and prioritizes the protection of the drive signal from being accidentally killed.

[0120] Furthermore, since the interference frequency (the instantaneous frequency of the main interference) is uncertain, sufficient bandwidth is needed to cover this fluctuation. At the same time, the system resonant frequency drifts with temperature, and bandwidth is also needed to cover the possible shift of the resonant frequency. Both of these require relatively large bandwidth. When the interference frequency is close to the resonant frequency, the protection requirements increase, and additional bandwidth needs to be reduced to avoid damaging the useful signal. Therefore, three constraints need to be constructed to obtain a suitable notch filter bandwidth.

[0121] In addition, the notch filter must fully cover the dynamic fluctuation range of the interference frequency; otherwise, some interference components will not be suppressed.

[0122] To construct the above three constraints, a continuous instantaneous frequency sequence of the main interference is obtained. Based on the instantaneous frequency sequence of the main interference, a sliding window with a size of 100 sampling points is set. For the current sampling time point t, the frequency standard deviation within the previous window is obtained, thus obtaining the interference fluctuation term (interference fluctuation degree) σf(t) of the interference frequency (instantaneous frequency of the main interference). The larger this value is, the more severe the interference frequency jitter, and the wider the notch bandwidth is required. Conversely, when this value is smaller, the interference frequency is stable, and a narrower bandwidth can be used.

[0123] Based on the absolute distance (distance difference) d(t) between the interference frequency and the resonant frequency, a Gaussian attenuation function is used to obtain the distance-related protection factor p(n) = exp(-d(t)). 2 At this point, when the absolute distance approaches infinity, the smaller the distance-related protection factor, the more maximum additional bandwidth can be used; when it approaches 0, increasing the bandwidth is prohibited; and when the absolute distance approaches 1, it is the larger of the -3dB bandwidth of the resonance peak and the preset minimum safe bandwidth (such as 0.1Hz).

[0124] This allows us to obtain the final target notch filter bandwidth: obtaining the interference fluctuation term σf(t) and the temperature drift term. f T The sum of (t); based on the positive and negative proportional relationship, obtain the difference between the distance-related protection factor p(n) and the value 1, i.e., 1-p(n);

[0125] Meanwhile, during the adjustment process, the adjustment range of the notch bandwidth is based on the system's design specifications. Here, the adjustment coefficient of the corresponding bandwidth is set to 1 under the distance-related protection factor, resulting in 1+(1-p(n)). From this, 1+(1-p(n)) is obtained along with the aforementioned disturbance fluctuation term σf(t) and temperature drift term. f T The target notch filter bandwidth BW(t) is obtained by multiplying the sums of (t) and (t).

[0126] The above process ultimately outputs the notch filter center frequency fd(t) and its bandwidth BW(t). It should be noted that, in order to ensure the accuracy of subsequent analysis, the minimum value of BW(t) is 1Hz. That is, when BW(t) is 0, a system error will be reported, or it will be forcibly adjusted to 1Hz.

[0127] Step S4: Determine the target filter using the target notch parameters, convert the drive state signal into a purified drive signal using the target filter, and control the gyroscope drive based on the purified drive signal.

[0128] Specifically, step S4, which involves determining the target filter using the target notch parameters and converting the drive state signal into a purified drive signal using the target filter, includes:

[0129] The digital center angular frequency is obtained using the center frequency of the target notch filter, and the pole radius factor is obtained using the bandwidth of the target notch filter.

[0130] The difference equation of the second-order IIR notch filter is obtained based on the digital center angular frequency and the pole radius factor to determine the corresponding target filter.

[0131] The filtered drive state signal is input into the target filter to convert the output into a purified drive signal.

[0132] In this embodiment, the target notch parameters calculated based on the above embodiments need to be suppressed by a specific filter. Since both the interference characteristics and the system state have time-varying characteristics, the filter needs to have real-time reconstruction capability. At the same time, the parameters calculated based solely on physical constraints cannot fully adapt to all complex dynamic scenarios, so it is necessary to evaluate the suppression performance of the corresponding filter in real time.

[0133] Based on the output notch filter center frequency and the final notch filter bandwidth, convert them into discrete-time filter coefficients (difference equation coefficients):

[0134] Obtain the system's fixed sampling frequency fs; based on the existing digital center angular frequency calculation method, obtain the digital center angular frequency 2π×fd(t) / fs corresponding to the current time t; obtain the pole radius factor 1-(π×(BW(t)) / fs);

[0135] Based on digital signal processing theory, the standard transfer function of a second-order IIR notch filter is determined by the digital center angular frequency and the pole radius factor, and the coefficients of its corresponding difference equation are also determined.

[0136] The filtered gyroscope signal S'(t) is used as input to the time-varying digital filter (target filter) described by the above difference equation.

[0137] The target filter operates in real time and outputs a purified gyroscope signal (purified drive signal) S''(t). This purified signal has significantly attenuated the narrowband interference components near the frequency fd(t) and retains the frequency estimated by the real-time resonant frequency f. res The useful signal of the driving mode centered on (t).

[0138] Specifically, step S4, which controls the gyroscope drive based on the purification drive signal, includes:

[0139] Determine the quality factor corresponding to the real-time resonant frequency, and use the filtered and smoothed quality factor to obtain the feedforward control quantity;

[0140] Extract the real-time vibration amplitude from the purification drive signal and determine the amplitude error between the real-time vibration amplitude and the expected amplitude.

[0141] The feedback control quantity is obtained by using the amplitude error, and the target driving voltage is obtained by combining the feedforward control quantity and the feedback control quantity to drive the gyroscope.

[0142] In this embodiment, for a second-order resonant system, the sharpness of the amplitude-frequency response curve of its transfer function near the resonant point is directly determined by the quality factor. The higher the quality factor, the sharper the resonant peak and the narrower the -3dB bandwidth; conversely, the lower the quality factor, the flatter the peak and the wider the bandwidth. Both satisfy the precise physical definition (existing definition): the quality factor equals the resonant frequency and the decrease in resonant peak value. The ratio between bandwidths.

[0143] The obtained real-time resonant frequency estimate f res Substituting (t) and the frequency range width into the existing calculation method for calculating the quality factor, the estimated real-time quality factor Q(t) corresponding to the real-time resonant frequency is obtained. This quality factor reflects the efficiency of the system in maintaining vibration and its sensitivity to external energy dissipation. The higher the quality factor, the smaller the driving force required to maintain the same amplitude, and the more sensitive the system is to interference within the same frequency band.

[0144] This embodiment uses the aforementioned purified gyroscope signal as the core feedback to perform high-precision drive closed-loop control, specifically:

[0145] Based on the system's established parameters, obtain the system's set constant target amplitude (i.e., expected amplitude), which refers to the preset output voltage amplitude of the drive detection electrode or the preset vibration displacement in the normal operating mode of the gyroscope.

[0146] The purified gyroscope signal S''(t) is input to a digital phase detector, which compares its phase with the current output of the internal numerically controlled oscillator to generate a phase error signal. This phase error is processed by a proportional-integral controller, which directly adjusts the frequency of the numerically controlled oscillator to maintain a fixed 90-degree phase difference between its output signal and the purified gyroscope signal S''(t), thereby precisely locking the driving frequency at the real-time resonant frequency f of the driving mode. res (t) on.

[0147] The real-time vibration amplitude is extracted from the purified gyroscope signal S''(t). The real-time vibration amplitude is compared with the system's preset constant target amplitude to obtain the amplitude error. The quality factor Q(t) calculated in real time is input into a low-pass filter or a moving average filter, and the smoothed quality factor Q'(t) is output as the feedforward input. Based on the physical relationship that the driving force required for the linear resonant system to maintain a constant amplitude at the resonance point is inversely proportional to the quality factor Q'(t), the feedforward control quantity is obtained.

[0148] The aforementioned amplitude error is processed by an independent proportional-integral controller to generate a feedback control quantity;

[0149] The feedforward control quantity is multiplied by the voltage conversion gain coefficient, and the product is added to the feedback control quantity to obtain the final target driving voltage, which is then applied to the gyroscope driving electrodes to achieve drive control. The voltage conversion gain coefficient is used to convert the dimensionless feedforward control quantity into a voltage signal, and its value can be, for example, 1V.

[0150] It should be noted that the feedforward channel directly responds to the change in the quality factor Q'(t). When the system damping increases, the quality factor Q'(t) decreases, and the driving voltage is automatically increased to offset the effect of increased energy loss in advance, so that the real-time amplitude can quickly return to and stabilize at a constant target amplitude.

[0151] Through the control optimization of this invention, a linear fit of 99.985% is achieved, and the zero-bias stability reaches 0.015 deg / h. Within the full range of ±100 deg / s, the system exhibits extremely high linear consistency. The measured nonlinear error is only 0.012% FS, proving the effectiveness of the multi-level automatic gain control and compensation algorithm. When subjected to a 15g mechanical shock, the system's detection loop exhibits good damping characteristics; the interference signal rapidly decays to zero within 100ms, without loop lock-up or parameter saturation.

[0152] This invention achieves precise location and frequency tracking of the primary vibration interference source by synchronously acquiring multi-source sensor data and selecting the axis with the highest energy among the three axes of the accelerometer as the main interference analysis axis. By integrating the three physical constraints of interference frequency fluctuation, temperature drift trend, and the relative distance between the interference frequency and the system resonant frequency, the optimal notch filter parameters are dynamically calculated. This allows the filter to adaptively cope with both the time-varying characteristics of vibration interference and the temperature drift of the gyroscope system (or inertial measurement system), ensuring effective suppression of interference while maximizing the protection of the useful drive signal. Finally, the purified drive signal is used for closed-loop control of the system, and feedforward compensation is performed in conjunction with the real-time estimated quality factor. This enables the system to actively and quickly offset the influence of damping changes on the drive force, thereby significantly improving the stability and rapid adaptive adjustment capability of the gyroscope drive mode, enhancing the gyroscope's accuracy in measuring angular velocity, and achieving high stability and robustness of the gyroscope drive mode in complex dynamic environments.

[0153] Example 2:

[0154] This invention also proposes a dynamic stabilization drive control device for a gyroscope. The device can be an inertial measurement unit, a mobile phone, a computer, a server, or other inertial and data analysis and computing devices, or a combination of multiple devices.

[0155] like Figure 6 As shown, Figure 6This is a schematic diagram of the hardware operating environment of the dynamic stabilization drive control device for a gyroscope, which is involved in the embodiments of the present invention.

[0156] like Figure 6 As shown, the dynamic stabilization drive control device for a gyroscope may include: a processor 1001, such as a CPU; a network interface 1004; a user interface 1003; a memory 1005; and a communication bus 1002. The communication bus 1002 is used to establish communication between these components. The user interface 1003 may include a display or an input unit such as a control panel; the user interface 1003 may also include a standard wired interface or a wireless interface. The network interface 1004 may optionally include a standard wired interface or a wireless interface (such as a Wi-Fi interface). The memory 1005 may be a high-speed RAM or a non-volatile memory, such as a disk drive. The memory 1005 may also optionally be a storage device independent of the aforementioned processor 1001. The memory 1005, as a computer storage medium, may include a dynamic stabilization drive control program for the gyroscope (hereinafter referred to as the "dynamic stabilization drive control program").

[0157] Those skilled in the art will understand that Figure 6 The hardware structure shown does not constitute a limitation on the device and may include more or fewer components than shown, or combine certain components, or have different component arrangements.

[0158] Continue to refer to Figure 6 , Figure 6 The memory 1005, which is a computer-readable storage medium, may include an operating device, a user interface module, a network communication module, and a dynamic stabilization drive control program for the gyroscope.

[0159] exist Figure 6 In this embodiment, the network communication module is mainly used to connect to the server and can communicate with the server for data; while the processor 1001 can call the dynamic stabilization drive control program for the gyroscope stored in the memory 1005 and execute the steps in the above embodiments.

[0160] Based on the hardware structure of the dynamic stabilization drive control device for a gyroscope described above, various embodiments of the dynamic stabilization drive control method for a gyroscope of the present invention are implemented.

[0161] Furthermore, the present invention also provides a dynamic stabilization drive control device for a gyroscope (hereinafter referred to as "dynamic stabilization drive control device"), please refer to... Figure 7 The dynamic stabilization drive control device for the gyroscope includes:

[0162] The signal acquisition module A10 is used to acquire the drive status signal and operating temperature signal of the gyroscope, determine the dominant interference axis based on the triaxial acceleration signal of the triaxial accelerometer, and obtain the main interference reference signal.

[0163] The signal analysis module A20 is used to perform power spectrum analysis on the drive state signal to obtain the real-time resonant frequency, determine the degree of temperature drift based on the operating temperature signal, determine the instantaneous frequency of the main interference relative to the nominal drive frequency of the gyroscope, determine the distance difference between the real-time resonant frequency and the instantaneous frequency of the main interference, and determine the target notch parameters of the filter by combining the degree of temperature drift, the instantaneous frequency of the main interference and the distance difference.

[0164] The drive parameter tuning module A30 is used to determine the target filter using the target notch parameters, convert the drive state signal into a purified drive signal through the target filter, and control the gyroscope drive based on the purified drive signal.

[0165] Furthermore, the signal acquisition module A10 is also used for:

[0166] Acquire the mean square values ​​of the triaxial acceleration signals from the triaxial accelerometer within a preset time window;

[0167] The axis corresponding to the maximum mean square value is taken as the dominant disturbance axis among the three axes, and the corresponding acceleration signal is taken as the main disturbance reference signal.

[0168] Furthermore, the signal analysis module A20 is also used for:

[0169] The original drive state signal is input into a zero-phase FIR bandpass filter to obtain a filtered drive signal;

[0170] Power spectrum analysis is performed on multiple consecutive filtered drive signal segments to obtain various power spectrum curves.

[0171] The driving peak frequencies in each power spectrum curve are obtained based on the spectral peak characteristics that distinguish the driving signal from the interference signal.

[0172] The real-time resonant frequency is obtained by performing a moving average filter on the peak frequencies of each driving spectrum.

[0173] Furthermore, the signal analysis module A20 is also used for:

[0174] Determine the operating temperature sequence of the operating temperature signal within the most recent time window, and determine the correlation coefficient and variation relationship between the operating temperature and the resonant frequency in the operating temperature sequence;

[0175] Determine the current average rate of change of the operating temperature over time at the current moment, and use the correlation coefficient, the relationship of change, and the current average rate to determine the degree of current temperature drift.

[0176] Furthermore, the signal analysis module A20 is also used for:

[0177] The corresponding linear phase FIR filter is determined based on the gyroscope's nominal drive frequency and the preset maximum expected frequency deviation.

[0178] The main interference reference signal is input into the linear phase FIR filter to obtain the filtered main interference reference signal;

[0179] The instantaneous frequency of the main interference is obtained by determining the instantaneous frequency offset of the main interference reference signal after demodulation and filtering using the nominal driving frequency of the gyroscope as the reference frequency and the instantaneous phase offset of the main interference reference signal relative to the nominal driving frequency of the gyroscope.

[0180] Furthermore, the signal analysis module A20 is also used for:

[0181] Determine the power spectrum curve corresponding to the real-time resonant frequency, and based on the power spectrum curve, determine the frequency range width corresponding to when the peak amplitude of the resonant peak drops to half the power bandwidth;

[0182] The absolute value of the difference between the real-time resonant frequency and the instantaneous frequency of the main interference is determined, and the distance difference is determined by the ratio between the absolute value of the difference and the width of the frequency range.

[0183] Furthermore, the signal analysis module A20 is also used for:

[0184] The instantaneous frequency of the main interference is used as the target notch filter center frequency of the filter, and the interference fluctuation degree of the main interference instantaneous frequency is determined by the sequence of the main interference instantaneous frequency.

[0185] A distance-dependent protection factor is constructed using the aforementioned distance difference, which is negatively correlated with the notch filter bandwidth design range.

[0186] The target notch filter bandwidth is obtained by combining the degree of temperature drift, the degree of interference fluctuation, and the distance-related protection factor.

[0187] Furthermore, the drive parameter tuning module A30 is also used for:

[0188] The digital center angular frequency is obtained using the center frequency of the target notch filter, and the pole radius factor is obtained using the bandwidth of the target notch filter.

[0189] The difference equation of the second-order IIR notch filter is obtained based on the digital center angular frequency and the pole radius factor to determine the corresponding target filter.

[0190] The filtered drive state signal is input into the target filter to convert the output into a purified drive signal.

[0191] Furthermore, the drive parameter tuning module A30 is also used for:

[0192] Determine the quality factor corresponding to the real-time resonant frequency, and use the filtered and smoothed quality factor to obtain the feedforward control quantity;

[0193] Extract the real-time vibration amplitude from the purification drive signal and determine the amplitude error between the real-time vibration amplitude and the expected amplitude.

[0194] The feedback control quantity is obtained by using the amplitude error, and the target driving voltage is obtained by combining the feedforward control quantity and the feedback control quantity to drive the gyroscope.

[0195] The specific implementation of the dynamic stabilization drive control device for gyroscopes of the present invention is basically the same as the embodiments of the dynamic stabilization drive control method for gyroscopes described above, and will not be repeated here.

[0196] Furthermore, the present invention also provides a computer-readable storage medium. The computer-readable storage medium stores a dynamic stabilization drive control program for a gyroscope, wherein when the dynamic stabilization drive control program for a gyroscope is executed by a processor, it implements the steps of the dynamic stabilization drive control method for a gyroscope as described above.

[0197] The method implemented when the dynamic stabilization drive control program for the gyroscope is executed can be referred to in various embodiments of the dynamic stabilization drive control method for the gyroscope of the present invention, and will not be repeated here.

[0198] It should be noted that the order of the above embodiments of the present invention is merely for descriptive purposes and does not represent the superiority or inferiority of the embodiments. The processes depicted in the accompanying drawings do not necessarily require a specific or sequential order to achieve the desired result. In some embodiments, multitasking and parallel processing are also possible or may be advantageous.

[0199] The various embodiments in this specification are described in a progressive manner. The same or similar parts between the various embodiments can be referred to each other. Each embodiment focuses on describing the differences from other embodiments.

[0200] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, apparatus, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0201] The above description is only a preferred embodiment of the present invention and does not limit the scope of protection of the present invention. All equivalent structural / method transformations made under the inventive concept of the present invention using the contents of the present invention specification and drawings, or direct / indirect applications in other related technical fields, are included within the scope of protection of the present invention.

Claims

1. A dynamic stabilization drive control method for a gyroscope, characterized in that, The method includes: The drive status signal and operating temperature signal of the gyroscope are acquired, and the dominant interference axis is determined based on the triaxial acceleration signal of the triaxial accelerometer and the main interference reference signal is obtained. The real-time resonant frequency is obtained by performing power spectrum analysis on the drive state signal. The degree of temperature drift is determined based on the operating temperature signal, and the instantaneous frequency of the main interference reference signal relative to the gyroscope's nominal drive frequency is determined. Determining the degree of temperature drift based on the operating temperature signal includes: determining the operating temperature sequence within the most recent time window; determining the correlation coefficient and change relationship between the operating temperature and the resonant frequency in the operating temperature sequence; determining the current average rate of change of the operating temperature over time at the current moment; and determining the current degree of temperature drift using the correlation coefficient, change relationship, and current average rate. Determining the instantaneous frequency of the main interference reference signal relative to the gyroscope's nominal drive frequency includes: determining the corresponding linear-phase FIR filter based on the gyroscope's nominal drive frequency and a preset maximum expected frequency offset; inputting the main interference reference signal into the linear-phase FIR filter to obtain the filtered main interference reference signal; demodulating the filtered main interference reference signal with the gyroscope's nominal drive frequency as the reference frequency; and determining the instantaneous frequency offset of its instantaneous phase relative to the gyroscope's nominal drive frequency to obtain the instantaneous frequency of the main interference. The process involves determining the distance difference between the real-time resonant frequency and the instantaneous frequency of the main interference, and then determining the target notch parameters of the filter by combining the degree of temperature drift, the instantaneous frequency of the main interference, and the distance difference. This determination includes: using the instantaneous frequency of the main interference as the target notch filter center frequency; determining the interference fluctuation degree of the main interference instantaneous frequency using the main interference instantaneous frequency sequence; constructing a distance-related protection factor that is negatively correlated with the notch filter bandwidth design range using the distance difference; and obtaining the target notch filter bandwidth by combining the degree of temperature drift, the degree of interference fluctuation, and the distance-related protection factor. The determination of the distance difference between the real-time resonant frequency and the instantaneous frequency of the main interference includes: determining the power spectrum curve corresponding to the real-time resonant frequency; determining the frequency range width corresponding to the peak amplitude of the resonant peak dropping to half the power bandwidth based on the power spectrum curve; determining the absolute value of the difference between the real-time resonant frequency and the instantaneous frequency of the main interference; and determining the distance difference by using the ratio of the absolute value of the difference to the frequency range width. The target filter is determined using the target notch parameters. The drive state signal is converted into a purified drive signal through the target filter. The gyroscope drive is then controlled based on the purified drive signal.

2. The dynamic stabilization drive control method for a gyroscope according to claim 1, characterized in that, The determination of the dominant interference axis and the acquisition of the main interference reference signal based on the triaxial acceleration signal from the triaxial accelerometer includes: Acquire the mean square values ​​of the triaxial acceleration signals from the triaxial accelerometer within a preset time window; The axis corresponding to the maximum mean square value is taken as the dominant disturbance axis among the three axes, and the corresponding acceleration signal is taken as the main disturbance reference signal.

3. The dynamic stabilization drive control method for a gyroscope according to claim 1, characterized in that, The step of performing power spectrum analysis on the driving state signal to obtain the real-time resonant frequency includes: The original drive state signal is input into a zero-phase FIR bandpass filter to obtain a filtered drive signal; Power spectrum analysis is performed on multiple consecutive filtered drive signal segments to obtain various power spectrum curves. The driving peak frequencies in each power spectrum curve are obtained based on the spectral peak characteristics that distinguish the driving signal from the interference signal. The real-time resonant frequency is obtained by performing a moving average filter on the peak frequencies of each driving spectrum.

4. The dynamic stabilization drive control method for a gyroscope according to claim 1, characterized in that, The step of determining the target filter using the target notch parameters and converting the drive state signal into a purified drive signal using the target filter includes: The digital center angular frequency is obtained using the center frequency of the target notch filter, and the pole radius factor is obtained using the bandwidth of the target notch filter. The difference equation of the second-order IIR notch filter is obtained based on the digital center angular frequency and the pole radius factor to determine the corresponding target filter. The filtered drive state signal is input into the target filter to convert the output into a purified drive signal.

5. The dynamic stabilization drive control method for a gyroscope according to claim 1, characterized in that, The gyroscope drive control based on the purification drive signal includes: Determine the quality factor corresponding to the real-time resonant frequency, and use the filtered and smoothed quality factor to obtain the feedforward control quantity; Extract the real-time vibration amplitude from the purification drive signal and determine the amplitude error between the real-time vibration amplitude and the expected amplitude. The feedback control quantity is obtained by using the amplitude error, and the target driving voltage is obtained by combining the feedforward control quantity and the feedback control quantity to drive the gyroscope.

6. A dynamic stabilization drive control device for a gyroscope, characterized in that, The device is used to implement the dynamic stabilization drive control method for a gyroscope as described in any one of claims 1 to 5; the device includes: The signal acquisition module is used to acquire the drive status signal and operating temperature signal of the gyroscope, and to determine the dominant interference axis and obtain the main interference reference signal based on the triaxial acceleration signal of the triaxial accelerometer. The signal analysis module is used to perform power spectrum analysis on the drive state signal to obtain the real-time resonant frequency, determine the degree of temperature drift based on the operating temperature signal, determine the instantaneous frequency of the main interference relative to the nominal drive frequency of the gyroscope, determine the distance difference between the real-time resonant frequency and the instantaneous frequency of the main interference, and determine the target notch parameters of the filter by combining the degree of temperature drift, the instantaneous frequency of the main interference and the distance difference. The drive parameter tuning module is used to determine the target filter using the target notch parameters, convert the drive state signal into a purified drive signal through the target filter, and control the gyroscope drive based on the purified drive signal.

Images