Resonator gyroscope nonlinear error identification and compensation method and system based on multi-harmonic coherent demodulation

Abstract

The present application relates to a kind of resonance gyroscope nonlinear error identification and compensation method and system based on multi-harmonic coherent demodulation, method includes: step 1, using the base frequency signal of frequency-locked loop, 2-6 harmonic reference signals are generated by frequency multiplication;Step 2, using coherent demodulation and band-stop filter to extract harmonic component, wherein, using band-stop filter to filter the interference component of two frequency produced by demodulation;Step 3, the same direction component amplitude and quadrature component amplitude obtained by demodulation are respectively input into two independent PID controllers for correction, obtain two compensation signals, modulate two compensation signals with the harmonic reference signal generated in step 1, after superposition, conversion and amplification, the compensation signal after modulation is fed back to gyro drive end, form closed-loop compensation.The present application realizes high-precision harmonic extraction, realizes independent accurate control, realizes comprehensive compensation, realizes closed-loop self-adaption: real-time tracking harmonic change, adapt to temperature, pressure and other environmental fluctuations, and has the advantages of low implementation cost.

Description

Technical Field

[0001] This invention relates to the field of inertial navigation and precision instrument technology, specifically to a method and system for identifying and compensating nonlinear errors in resonant gyroscopes based on multi-harmonic coherent demodulation, which is particularly applicable to resonant angular rate sensors such as hemispherical resonant gyroscopes (HRG) and micromechanical system (MEMS) gyroscopes. Background Technology

[0002] The performance of a resonant gyroscope is severely limited by the nonlinear errors in the detection and control circuits, which mainly originate from two aspects: the physical nonlinearity of capacitance detection and the system nonlinearity of the detection circuit.

[0003] 1. Capacitive sensing nonlinearity: This is an inherent physical error in resonant gyroscopes (such as HRG and MEMS gyroscopes). The variable capacitance formed between the resonator and the electrodes is related as follows: (Where x is the vibration displacement). The Taylor expansion of this relationship contains high-order nonlinear terms (second, third, and higher), resulting in significant 2nd to nth harmonic components in the detection signal. These harmonic interferences are one of the core factors causing gyroscope zero-bias drift and nonlinearity of the scale factor.

[0004] 2. Detection circuit nonlinearity: Even if the capacitor signal is ideal, the electronic components in the subsequent signal chain will introduce nonlinearity.

[0005] Operational amplifier nonlinearity: Op-amps used for signal conditioning suffer from problems such as output swing limitation, input offset voltage temperature drift, and open-loop gain nonlinearity, especially when processing weak signals, introducing DC offset and gain error.

[0006] Data converter nonlinearity: Integral nonlinearity (INL) and differential nonlinearity (DNL) of AD converters (ADCs) and DA converters (DACs) directly lead to harmonic distortion and measurement errors. Noise and drift of the reference voltage source also affect conversion linearity.

[0007] The modulation and demodulation process is nonlinear: In coherent demodulation, if the purity of the reference signal itself is insufficient (such as the presence of harmonic spurious signals), it will cross-modulate with the input signal, contaminating the DC component of the demodulated output.

[0008] Existing technologies mostly employ open-loop estimation or single harmonic suppression (such as compensating only the second harmonic), which suffers from problems such as incomplete compensation, poor adaptability, and susceptibility to environmental interference. Some analog compensation circuits are greatly affected by temperature changes and lack stability. Therefore, a method is needed that can accurately identify multi-order harmonics, suppress the nonlinear effects of circuits, and achieve closed-loop adaptive compensation. Summary of the Invention

[0009] To address the shortcomings of existing technologies, this invention proposes a method and system for identifying and compensating nonlinear errors in resonant gyroscopes based on multi-harmonic coherent demodulation.

[0010] One of the above-mentioned objectives of the present invention is achieved by the following technical solution:

[0011] A method for identifying and compensating for nonlinear errors in resonant gyroscopes based on multi-harmonic coherent demodulation includes the following steps:

[0012] Step 1: Using the fundamental frequency signal of the frequency-locked loop, generate 2nd to 6th harmonic reference signals by frequency multiplication;

[0013] Step 2: Harmonic components are extracted using coherent demodulation and band-stop filtering, wherein a band-stop filter is used to filter out the second harmonic interference components generated by demodulation.

[0014] Step 3: Input the amplitude of the same-direction component and the amplitude of the quadrature component obtained by demodulation into two independent PID controllers for correction to obtain two compensation signals. Modulate the two compensation signals with the harmonic reference signal generated in step 1. After superimposing, converting and amplifying the quality-modulated compensation signals, feed them back to the gyroscope drive to form a closed-loop compensation.

[0015] Moreover, the reference harmonic signal generated in step 1 is implemented by the DDS module, and its output reference signal has a spurious-free dynamic range better than 80 dBc.

[0016] Furthermore, the center frequency of the band-stop filter is set to twice the frequency of the coherent demodulation reference signal, its stopband bandwidth is set to 1%~5% of the fundamental frequency, and its stopband suppression is greater than 60dB.

[0017] Moreover, the proportional, integral, and derivative parameters of the two independent PID controllers are tuned independently.

[0018] Furthermore, in step 3, the outputs of the two PID controllers are multiplied by the corresponding reference harmonic signals to achieve signal modulation.

[0019] Furthermore, in step 3, the generated compensation signal is applied to the gyroscope electrodes through a high-linearity DA converter and amplifier circuit.

[0020] The second objective of this invention is achieved through the following technical solution:

[0021] A system for implementing the above-mentioned method for identifying and compensating nonlinear errors in resonant gyroscopes based on multi-harmonic coherent demodulation includes:

[0022] The system includes a main control chip, an analog-to-digital converter (ADC), a digital-to-analog converter (DAC), and an operational amplifier. The main control chip uses an FPGA or a high-performance DSP as its core. The main control chip contains a DDS module, a multiplier, a band-stop filter module, a dual-PID correction network module, and a modulator, used to implement harmonic reference signal generation, multiplication demodulation, band-stop filtering, and dual-PID correction digital algorithms. The dual-PID correction network module consists of two independent PID controllers.

[0023] Furthermore, the analog-to-digital converter (ADC) is a 16-bit or higher high-resolution device.

[0024] Moreover, the digital-to-analog converter (DAC) is a high-linearity DA converter with an integral nonlinearity (INL) value better than ±2LSB.

[0025] The advantages and positive effects of this invention are as follows:

[0026] 1. High-precision harmonic extraction is achieved: a band-stop filter is used to accurately filter out the second harmonic component after demodulation, and a high SFDR of the DDS reference signal is required. This dual guarantee from the signal source and extraction stage greatly improves the accuracy of harmonic amplitude detection.

[0027] 2. Achieved independent and precise control: Adopting a dual PID parallel control architecture, it independently processes the same-direction and quadrature components of harmonics, which can more precisely eliminate errors in specific phases and achieve better compensation results.

[0028] 3. Comprehensive compensation is achieved: covering the 2nd to nth harmonics, significantly reducing the combined nonlinear error caused by capacitive nonlinearity and circuit nonlinearity.

[0029] 4. Achieved closed-loop adaptive operation: Real-time tracking of harmonic changes to adapt to environmental fluctuations such as temperature and pressure.

[0030] 5. Low implementation cost: Utilizing existing frequency locking circuits and DDS modules, no additional detection circuits are required. Attached Figure Description

[0031] Figure 1 This is a schematic diagram of the nonlinear error identification and compensation system for resonant gyroscopes based on multi-harmonic coherent demodulation according to the present invention;

[0032] Figure 2 This is a schematic diagram of the harmonic compensation algorithm implemented inside the FPGA of this invention. Detailed Implementation

[0033] The structure of the present invention will be further described below with reference to the accompanying drawings and embodiments.

[0034] A method for identifying and compensating nonlinear errors in resonant gyroscopes based on multi-harmonic coherent demodulation is proposed. The invention lies in the following: This method utilizes the fundamental frequency signal of a frequency-locked loop to generate a high-purity reference harmonic, extracts the harmonic amplitude through coherent demodulation, filters out second-harmonic interference using a band-stop filter, and finally achieves closed-loop compensation through dual PID correction and modulation feedback. The system block diagram is shown below. Figure 1 As shown, the specific steps are as follows:

[0035] Step 1: Generation of high-purity harmonic reference signal

[0036] The high-purity fundamental frequency signals sin(ωt) and cos(ωt) are output from the existing frequency-locking circuit of the gyroscope (such as PLL or self-excited oscillation circuit).

[0037] The reference signals for the 2nd to nth harmonics are generated using a high-performance direct digital frequency synthesizer (DDS) module: sin(2ωt), cos(2ωt), sin(3ωt), cos(3ωt), ..., sin(nωt), cos(nωt).

[0038] The purity requirements for DDS reference signals are as follows: To ensure accurate harmonic identification, the DDS-generated reference signals for each harmonic must have high spectral purity, with a spurious-free dynamic range (SFDR) significantly higher than the theoretical amplitude of the harmonic signal under test, exceeding 80 dBc. If the reference signal itself contains nonlinear distortion, such as containing an m-th harmonic (m ≠ k), this distortion component will coherently demodulate with the m-th harmonic component in the input signal Z(t), generating an additional DC error component that mixes into the amplitude measurement result of the k-th harmonic, leading to identification errors. Therefore, a high SFDR DDS reference signal is a prerequisite for ensuring that the amplitudes of each harmonic are extracted independently and accurately.

[0039] Step 2: Coherent demodulation and band-stop filtering to extract harmonic components

[0040] The gyroscope detection signal (including nonlinear error) acquired by the AD converter is multiplied with each harmonic reference signal (coherent demodulation), as shown in the following formula:

[0041]

[0042] Where k = 2,3,4….n.

[0043] The signal output by the multiplier contains a DC component (the desired harmonic amplitude) and a second harmonic component (2kΩ). To accurately extract the DC component, second harmonic interference needs to be filtered out. This invention uses a band-stop filter instead of a traditional low-pass filter, with its center frequency precisely set to 2kΩ.

[0044] After passing through a band-stop filter, the DC component is obtained:

[0045] A_sin = Notch_Filter[I(t)] / / Amplitude of the k-th harmonic component in the same direction

[0046] A_cos = Notch_Filter[Q(t)] / / Amplitude of the quadrature component of the k-th harmonic

[0047] This yields the amplitudes of the same-direction and quadrature components of the k-th harmonic. Compared to traditional low-pass filters, band-stop filters offer a higher stopband rejection ratio and a steeper transition band, enabling more effective extraction of the DC component and improving the accuracy of harmonic amplitude detection.

[0048] Step 3: PID correction network and modulation feedback

[0049] The amplitudes A_sin and A_cos are input into two independent PID controllers (e.g., PID_sin and PID_cos) respectively, and parallel calculations are performed to generate their respective compensation signals.

[0050] Comp_sin(t) = Kp_s × A_sin + Ki_s × ∫A_sin dt + Kd_s × dA_sin / dt

[0051] Comp_cos(t) = Kp_c × A_cos + Ki_c × ∫A_cos dt + Kd_c × dA_cos / dt

[0052] This dual PID architecture can independently and precisely control the in-phase and quadrature components of the harmonic signal, achieving more refined error compensation.

[0053] The reference harmonic signal generated by DDS is used to modulate the two compensation signals respectively:

[0054] Output_sin(t) = Comp_sin(t) × sin(kωt)

[0055] Output_cos(t) = Comp_cos(t) × cos(kωt)

[0056] The total compensation signal, composed of all harmonics (k=2~n), is superimposed and fed back to the gyroscope drive electrode via a high-linearity DAC and amplifier circuit, forming a closed-loop compensation. The high-linearity DAC effectively reduces the second-order nonlinear distortion introduced by the compensation signal feedback loop.

[0057] The compensation system for implementing the above-mentioned method for identifying and compensating nonlinear errors in resonant gyroscopes based on multi-harmonic coherent demodulation mainly includes a main control chip, an analog-to-digital converter (ADC), a digital-to-analog converter (DAC), and an operational amplifier. The main control chip uses an FPGA or a high-performance DSP as its core, and it integrates a DDS module, a multiplier, a band-stop filter module, a dual-PID correction network module, and a modulator to implement digital algorithms such as DDS generation, multiplication demodulation, band-stop filtering, and dual-PID correction. Specifically:

[0058] DDS Module: Select a high-performance DDS chip or IP core, whose SFDR should be better than 80 dBc within the target output frequency (2ω~nω). Combine this with a sine function compression algorithm and noise shaping technology to reduce amplitude quantization error and phase truncation noise.

[0059] Band-stop filter module design:

[0060] Type: Implementing a second-order IIR notch filter or a higher-order FIR filter in the digital domain at its center frequency. Strictly align with the second harmonic point after demodulation of each harmonic, i.e. .

[0061] Bandwidth: Usually set to baseband 1% to 5%.

[0062] Stopband suppression: Requires >60dB.

[0063] Operational amplifier: Select a precision op-amp with low offset, low drift, and high open-loop gain to reduce the inherent nonlinearity of the signal conditioning stage.

[0064] The analog-to-digital converter (ADC) uses 16-bit or higher high-resolution devices to ensure sampling accuracy. The digital-to-analog converter (DAC) uses a high-linearity DA converter, where the typical integral nonlinearity (INL) is better than ±2 LSB, ensuring the linearity of the compensation signal feedback loop.

[0065] Although embodiments and drawings of the present invention have been disclosed for illustrative purposes, those skilled in the art will understand that various substitutions, variations and modifications are possible without departing from the spirit and scope of the present invention and the appended claims. Therefore, the scope of the present invention is not limited to the contents disclosed in the embodiments and drawings.

Claims

1. A method for identifying and compensating nonlinear errors in a resonant gyroscope based on multi-harmonic coherent demodulation, characterized in that, Includes the following steps: Step 1: Using the fundamental frequency signal of the frequency-locked loop, generate 2nd to 6th harmonic reference signals by frequency multiplication; Step 2: Harmonic components are extracted using coherent demodulation and band-stop filtering. A band-stop filter is used to filter out the second harmonic interference components generated by demodulation. The center frequency of the band-stop filter is set to twice the frequency of the coherent demodulation reference signal, its stopband bandwidth is set to 1%~5% of the fundamental frequency, and the stopband rejection is greater than 60dB. Step 3: Input the amplitude of the same-direction component and the amplitude of the quadrature component obtained by demodulation into two independent PID controllers for correction to obtain two compensation signals. Modulate the two compensation signals with the harmonic reference signal generated in step 1. After superimposing, converting and amplifying the quality-modulated compensation signals, feed them back to the gyroscope drive to form a closed-loop compensation. The reference harmonic signal generated in step 1 is implemented by the DDS module, and its output reference signal has a spurious-free dynamic range better than 80dBc. The proportional, integral, and derivative parameters of the two independent PID controllers are tuned independently.

2. The method for identifying and compensating nonlinear errors in resonant gyroscopes based on multi-harmonic coherent demodulation according to claim 1, characterized in that: In step 3, the outputs of the two PID controllers are multiplied by the corresponding reference harmonic signals to achieve signal modulation.

3. The method for identifying and compensating nonlinear errors in resonant gyroscopes based on multi-harmonic coherent demodulation according to claim 1, characterized in that: In step 3, the generated compensation signal is applied to the gyroscope electrodes through a high-linearity DA converter and amplifier circuit.

4. A system for implementing the nonlinear error identification and compensation method for resonant gyroscopes based on multi-harmonic coherent demodulation as described in any one of claims 1-3, characterized in that, include: The system includes a main control chip, an analog-to-digital converter (ADC), a digital-to-analog converter (DAC), and an operational amplifier. The main control chip uses an FPGA or a high-performance DSP as its core. The main control chip contains a DDS module, a multiplier, a band-stop filter module, a dual-PID correction network module, a modulator, and an operational amplifier. These components are used to implement harmonic reference signal generation, multiplication demodulation, band-stop filtering, and dual-PID correction digital algorithms. The dual-PID correction network module consists of two independent PID controllers.

5. The system according to claim 4, characterized in that: The analog-to-digital converter (ADC) is a high-resolution device with 16 bits or more.

6. The system according to claim 4, characterized in that, The digital-to-analog converter (DAC) is a high-linearity DA converter with an integral nonlinearity (INL) value better than ±2LSB.

Images