All-digital autonomous frequency control method for hemispherical resonator gyroscope

Abstract

The invention discloses an all-digital autonomous frequency control method for a hemispherical resonator gyroscope, and belongs to the technical field of inertial instrument control. The method comprises the following steps: firstly, carrying out autonomous identification on the frequency of the hemispherical resonator gyroscope, and then carrying out full-digital closed-loop control on the hemispherical resonator gyroscope. The automatic identification of the frequency of the hemispherical resonator gyroscope is realized by adopting an active excitation and signal frequency identification mode, and the full-digital closed-loop control of the hemispherical resonator gyroscope comprises digital frequency and phase detection, digital loop filtering, numerical control vibrator processing and digital reference signal source output. According to the method, autonomous identification of the initial frequency of the hemispherical resonator gyroscope and high-stability and high-precision frequency closed-loop control can be realized, and the production efficiency and the performance index of the hemispherical resonator gyroscope product are effectively improved.

Description

Technical Field

[0001] The invention relates to a full-digital autonomous frequency control method for a hemispherical resonant gyroscope, belonging to the technical field of inertial instrument control. Background Art

[0002] The control of hemispherical resonant gyroscope is generally divided into frequency control, amplitude control, orthogonal control and force balance control, among which frequency control is the core and foundation of hemispherical resonant gyroscope control. Frequency control provides the synchronous frequency and phase reference for amplitude control, orthogonal control and force balance control. In view of the control problem of hemispherical resonant gyroscope, a frequency control method suitable for analog control mode has been studied and used for frequency control of hemispherical resonant gyroscope, and it has been applied in model products, solving the frequency control problem of the first generation of hemispherical resonant gyroscope products. However, with the development of hemispherical resonant gyroscope technology and the demand for product upgrading, hemispherical resonant gyroscope control has been upgraded from analog control mode to full digital control mode. The previously proposed frequency control method is no longer applicable, and a new frequency control method is needed. At the same time, the requirements for scale of production have been improved, and higher requirements have been put forward for the production efficiency of hemispherical resonant gyroscope products. Frequency control, as the core link of hemispherical resonant gyroscope control, must implement the scale requirements, and the frequency control link must be fully autonomous in operation to improve the production efficiency of hemispherical resonant gyroscope products. Summary of the invention

[0003] The technical problem solved by the present invention is: to overcome the shortcomings of the prior art, a fully digital autonomous frequency control method for a hemispherical resonant gyroscope is proposed, and a fully digital closed-loop control is adopted. The generated reference signal can be used as a digital reference signal for amplitude control, orthogonal control and force balance control links, thereby realizing stable, accurate and environmentally adaptable frequency control and phase control.

[0004] The technical solution of the present invention is: A fully digital autonomous frequency control method for a hemispherical resonant gyroscope, comprising: After the output signal of the hemispherical resonant gyro is converted by ADC, the frequency of the hemispherical resonant gyro is identified as the initial value of the frequency control of the hemispherical resonant gyro; The signal converted by ADC is processed with frequency and phase discrimination, and the frequency and phase of the output signal are compared with the reference signal to obtain the phase difference signal; Performing digital loop filtering on the phase difference signal to obtain a filtered voltage signal; The initial value of the hemispherical resonant gyro frequency control is used as the initial frequency of the digital control oscillator, the voltage signal is used as input, and the frequency signal is output; The frequency signal is input into a digital reference source to generate a reference signal and input it into a frequency and phase detector to realize closed-loop control of the frequency of the hemispherical resonant gyroscope. The reference signal after closed-loop control is derived and applied to the amplitude control, orthogonal control and force balance control of the hemispherical resonant gyroscope.

[0005] Furthermore, the method for identifying the frequency of the hemispherical resonant gyroscope includes: S1: ADC samples the output signal of the hemispherical resonant gyro, and counts the number of zero crossings of the sampled data within two consecutive seconds in units of one second. The count value is the identification frequency value; S2: Determine the correctness of the two initial frequency values: and and

[0006] In the formula, and are the minimum and maximum values ​​of the determination frequency, respectively. and are the two adjacent identification frequency values, is the determination threshold of the difference; If the determination is passed, S3 is executed; if the determination is not passed, the hemispherical resonant gyroscope is actively excited. When the amplitude of the output signal of the hemispherical resonant gyroscope is greater than the set threshold value, the excitation is stopped, and S1 and S3 are executed in sequence; S3: averaging two adjacent identified frequency values, and using the average value as the frequency of the hemispherical resonant gyroscope.

[0007] Furthermore, if the determination fails, the hemispherical resonant gyroscope is actively excited, and the method of active excitation is: The hemispherical resonant gyroscope is excited by a variable frequency sinusoidal signal. The frequency of the sinusoidal signal changes from an initial value to Gradually increase to the final frequency value ; An active excitation circuit is constructed, including an ADC circuit, a DAC circuit and a digital processing unit. The ADC circuit samples the output signal of the hemispherical resonant gyroscope, and the sampled digital signal is used as the input of the digital processing unit. The output of the digital processing unit is converted by the DAC circuit and then input into the hemispherical resonant gyroscope to form a ring loop; the digital processing unit is used to set the phase and gain. By setting the gain and phase, the loop gain is not less than 1, and the phase satisfies , is a non-negative integer, making the loop a positive feedback loop; when the amplitude of the hemispherical resonant gyroscope output signal sampled by the ADC current is greater than the set threshold value, the excitation is stopped.

[0008] Furthermore, for the variable-frequency sinusoidal signal, the step frequency of the frequency change is determined by the quality factor of the hemispherical resonant gyroscope, and the step time is determined by the resonant frequency of the hemispherical resonant gyroscope.

[0009] Furthermore, based on the mathematical model of frequency and phase discrimination, the output signal of the hemispherical resonant gyroscope is subjected to frequency and phase discrimination processing; the mathematical model is:

[0010]

[0011] In the formula, represents the state observation; It represents the reference signal used for frequency and phase discrimination, and its initial state is the identified hemispherical resonant gyro frequency; represents white noise, represents the observed white noise; represents the amplitude of the HRG output signal relative to the reference signal, It represents the phase difference of the HRG signal relative to the reference signal.

[0012] Furthermore, based on the mathematical model, the phase difference of the HRG output signal relative to the reference signal is obtained by recursive calculation: , specifically:

[0013] In the formula, express time The estimated value of express Always exist The estimated value of the moment, , for The intermediate variable at the moment, express Always exist The estimated value of the moment, , for and The covariance matrix of .

[0014] Furthermore, the phase difference signal is digitally loop filtered, which is expressed as:

[0015] In the formula, is the gain parameter, is the integration parameter, for The phase difference signal of the hemispherical resonant gyro signal relative to the reference signal at time instant, for The output signal obtained by digital loop filtering at this moment.

[0016] Furthermore, the frequency of the hemispherical resonant gyro is used as the initial frequency of the digital controlled oscillator, the voltage signal is used as the input, and the output frequency signal is for:

[0017] In the formula, is the filtered voltage signal, Represents the number of binary bits required to implement a numerically controlled oscillator, is the base frequency of the digital controlled oscillator, is the initial frequency of the numerically controlled oscillator.

[0018] Furthermore, the digital reference signal source is implemented using a CORDIC algorithm to generate a waveform signal of the output frequency signal of the digital controlled oscillator as a reference signal.

[0019] The advantages of the present invention compared with the prior art are: (1) The frequency autonomous identification method proposed in the present invention realizes the autonomous identification of the frequency of the hemispherical resonant gyroscope by using active excitation and timed one-second zero-crossing sampling. This method is an online frequency identification method and does not require offline measurement of the resonant frequency of the hemispherical resonant gyroscope. It significantly improves the debugging and production efficiency of the hemispherical resonant gyroscope products.

[0020] (2) The all-digital frequency control method proposed in the present invention uses an all-digital frequency and phase discrimination algorithm to recursively calculate the amplitude and phase of the gyro signal, and uses the phase information for frequency control. This method implements all-digital frequency closed-loop control in software mode, and is not easily affected by factors such as parameter aging drift of electronic devices on the frequency closed-loop control effect. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] Various other advantages and benefits will become apparent to those of ordinary skill in the art by reading the detailed description of the preferred embodiments below. The accompanying drawings are only for the purpose of illustrating the preferred embodiments and are not to be considered as limiting the present invention. Moreover, the same reference symbols are used throughout the accompanying drawings to represent the same components. In the accompanying drawings: Figure 1 This is a flow chart of a fully digital autonomous frequency control method for a hemispherical resonant gyroscope according to an embodiment of the present invention; Figure 2 This is a block diagram of the active excitation principle in the autonomous frequency identification of an embodiment of the present invention; Figure 3 This is a block diagram of the frequency closed-loop control principle in the frequency control of an embodiment of the present invention. DETAILED DESCRIPTION

[0022] The exemplary embodiments of the present disclosure will be described in more detail below with reference to the accompanying drawings. Although the exemplary embodiments of the present disclosure are shown in the accompanying drawings, it should be understood that the present disclosure can be implemented in various forms and should not be limited by the embodiments set forth herein. On the contrary, these embodiments are provided to enable a more thorough understanding of the present disclosure and to fully convey the scope of the present disclosure to those skilled in the art.

[0023] Aiming at the problems of insufficient automation in the debugging of hemispherical resonant gyroscope and low frequency control accuracy of hemispherical resonant gyroscope, the present invention proposes a fully digital autonomous frequency control method for hemispherical resonant gyroscope, such as Figure 1 As shown, specifically including: S1: Autonomous frequency identification of hemispherical resonant gyroscope S1.1: Frequency identification The sensitive signal of the hemispherical resonant gyro is generally a sinusoidal signal after being processed by the preamplifier circuit. The frequency identification of the hemispherical resonant gyro is to identify the sinusoidal signal that the hemispherical resonant gyro is sensitive to. Usually, the frequency of the hemispherical resonant gyro is between 4500Hz and 5500Hz. Frequency identification first involves ADC sampling of the sinusoidal electrical signal that the hemispherical resonant gyro is sensitive to. The sampling frequency is determined according to the Nyquist sampling theorem. Generally, a 10-fold sampling rate is preferably selected. In this embodiment, the sampling frequency is selected as 5MHz. Then, the number of zero crossings of the sampled data is counted in units of one second, and the count value is the identified frequency value. The identified frequency value is passed to S1.2 for frequency correctness determination.

[0024] S1.2: Frequency Correctness Determination The frequency correctness judgment is to eliminate the wild values ​​generated in the S1.1 frequency identification and ensure the correctness of the identified frequency.

[0025] Frequency correctness determination First determine the frequency value of frequency identification Whether it meets:

[0026] Among them, the minimum value of the determination frequency and maximum value According to the structural size and processing technology of the hemispherical resonator in the hemispherical resonator gyroscope, the frequency range of a 30mm hemispherical resonator is usually 4500Hz~5500Hz, so the minimum 4500Hz, maximum is 5500 Hz, that is, the embodiment determines whether the frequency value satisfies:

[0027] In order to ensure the effectiveness of eliminating outliers, the difference between the two consecutive identification frequency values ​​of S1.1 Make another judgment to determine whether the difference between two adjacent frequency identification frequency values ​​satisfies:

[0028] Among them, the difference determination threshold Determined by the resolution of S1.1 frequency identification, in this embodiment, a one-second timing is used for zero-crossing sampling counting, with a resolution of ±1Hz. Considering the margin of signal detection, the threshold is determined Select 2Hz.

[0029] If the following conditions are met, it is judged as "passed", otherwise, it is judged as failed.

[0030] and

[0031] After the judgment is "passed", the process goes to S1.4 to finalize the frequency. After the judgment is "failed", the process goes to S1.3 to actively excite the hemispherical resonant gyroscope.

[0032] S1.3: If S1.2 fails, the hemispherical resonant gyro is actively excited. S1.3.1: First, a variable frequency sinusoidal signal is used to excite the hemispherical resonant gyroscope. The frequency of the sinusoidal signal changes according to a certain step frequency. , a certain step time , from the initial frequency value Gradually increase to the final frequency value . The stepping frequency is determined by the quality factor of the hemispherical resonant gyroscope, the stepping time is determined by the resonant frequency of the hemispherical resonant gyroscope, and the initial value and final value of the frequency are determined by the structural size of the hemispherical resonator and the processing technology of the hemispherical resonator. In this embodiment, the quality factor of the hemispherical resonant gyroscope is 5 million and the resonant frequency is 5000Hz. The stepping frequency can be determined by the empirical formula (50~100)*5000Hz / 5000000, and the stepping frequency is 0.25Hz; the stepping time can be determined by the empirical formula (10~20)*1 / 5000, and the stepping time is 2ms. Usually the frequency range of a 30mm hemispherical resonator is 4500Hz~5500Hz, so the initial value of the frequency (also called the minimum value in S1.2) 4500Hz, the final frequency value (also called the maximum value in S1.2) It is 5500Hz.

[0033] S1.3.2: Construction Figure 2The active excitation circuit shown in the figure, where ADC is an analog-to-digital conversion circuit, DAC is a digital-to-analog conversion circuit, and the dotted box represents a digital processing unit, which is usually an FPGA or DSP circuit, and at least includes a gain setting and a phase setting link. By setting the gain and phase, the loop gain is not less than 1, and the phase satisfies According to the automatic control theory, when the above conditions are met, a positive feedback loop will be formed, and the hemispherical resonant gyro will be actively excited. When the ADC samples the amplitude of the sinusoidal signal of the hemispherical resonant gyro greater than the threshold value, the above excitation will be stopped, and the process will be transferred to S1.1, and the subsequent links will be carried out according to the process. In this embodiment, the gain is set to 10, the phase is set to 0, and the threshold value is set to 0.5V according to the empirical value.

[0034] S1.4: If S1.2 is passed, the average operation will be performed on the two adjacent identification frequency values ​​in S1.2. The average value of the two adjacent identification frequency values ​​will be used as the final determined frequency value and passed to S4 as the initial value of the operation.

[0035] S2: The purpose of the HRG frequency control is to synchronously track the HRG frequency and phase in real time. To achieve the above purpose, it is necessary to design a frequency closed-loop control circuit to perform closed-loop control of the HRG frequency and phase. Figure 3 It is a principle block diagram of the frequency control loop of a hemispherical resonant gyroscope, in which ADC is an analog-to-digital conversion circuit, and the dotted box represents a digital processing unit, which is usually an FPGA or DSP circuit. The closed-loop frequency control needs to include a frequency and phase detection link, a digital loop filter link, a digitally controlled oscillator link, and a digital reference signal source link.

[0036] The function of frequency and phase detection is to compare the frequency and phase difference between the sinusoidal signal of the hemispherical resonant gyroscope and the reference signal generated by the frequency closed-loop control. The purpose of the frequency closed-loop control loop is to control the sinusoidal signal of the hemispherical resonant gyroscope and the reference signal generated by the frequency closed-loop control to have the same phase and frequency. The difference in frequency between the two sinusoidal signals is essentially the phase difference, so frequency and phase detection is essentially to calculate the phase difference between the two sinusoidal signals.

[0037] In order to realize the calculation of phase difference, we first need to establish a mathematical model for frequency and phase detection:

[0038]

[0039] in, represents the calculated amplitude of the HRG signal relative to the reference signal, represents the resolved phase of the HRG signal relative to the reference signal, represents the state observation, It represents the reference signal for frequency and phase discrimination, which comes from the sine and cosine signals generated by S5. represents white noise, represents the observed white noise.

[0040] In order to realize the phase calculation of the hemispherical resonant gyro signal relative to the reference signal, the following mathematical calculation recursive formula is required:

[0041] in, represents the k moment The estimated value of represents the k-1 moment pair The estimated value at time k, , is the intermediate variable at time k, represents the k-1 moment pair The estimated value at time k, , for and The covariance matrix of is determined based on empirical values. In this embodiment, , .

[0042] Recursive solution It is the output of the frequency and phase detection and is passed to the S3 link.

[0043] S3: Digital loop filtering is essentially a control and adjustment link, which can be expressed by the formula:

[0044] in, is the gain parameter, is the integration parameter, is the output of S2 at time k , is the output of the digital loop filter at time k, which is passed to S4. Gain parameter and the integral parameter Through experimental debugging, in this embodiment , .

[0045] S4: The digital controlled oscillator is used to generate the frequency of the specified signal, which can be expressed as:

[0046] in, Output for S3 , Indicates the number of binary bits required to implement a numerically controlled oscillator, which is used to determine the resolution of the signal frequency. is the base frequency of the digital controlled oscillator, is the initial frequency of the numerically controlled oscillator, which comes from the frequency determination value of S1.4.

[0047] This embodiment takes , , achieving a frequency resolution of 4.55E-7Hz for the digital controlled oscillator.

[0048] S5: The digital reference signal source is implemented using the CORDIC algorithm and is used to generate a waveform signal of a specified frequency signal. The digital reference source receives the frequency signal generated by S4. , using the CORDIC algorithm to generate the reference signal required by S2 , .

[0049] The above-described embodiments are only preferred specific implementations of the present invention. Common changes and substitutions made by those skilled in the art within the scope of the technical solution of the present invention should be included in the protection scope of the present invention.

Claims

1. A fully digital autonomous frequency control method for a hemispherical resonant gyroscope, characterized in that: include: After the output signal of the hemispherical resonant gyro is converted by ADC, the frequency of the hemispherical resonant gyro is identified as the initial value of the frequency control of the hemispherical resonant gyro; The signal converted by ADC is processed with frequency and phase discrimination, and the frequency and phase of the output signal are compared with the reference signal to obtain the phase difference signal; Performing digital loop filtering on the phase difference signal to obtain a filtered voltage signal; The initial value of the hemispherical resonant gyro frequency control is used as the initial frequency of the digital control oscillator, the voltage signal is used as input, and the frequency signal is output; The frequency signal is input into a digital reference source to generate a reference signal and input it into a frequency and phase detector to realize closed-loop control of the frequency of the hemispherical resonant gyroscope. The reference signal after closed-loop control is derived and applied to the amplitude control, orthogonal control and force balance control of the hemispherical resonant gyroscope.

2. The fully digital autonomous frequency control method of a hemispherical resonant gyroscope according to claim 1, characterized in that: Methods for identifying the frequency of a HRG include: S1: ADC samples the output signal of the hemispherical resonant gyro, and counts the number of zero crossings of the sampled data within two consecutive seconds in units of one second. The count value is the identification frequency value; S2: Determine the correctness of the two initial frequency values: and and In the formula, and are the minimum and maximum values ​​of the determination frequency, respectively. and are the two adjacent identification frequency values, is the determination threshold of the difference; If the determination is passed, S3 is executed; if the determination is not passed, the hemispherical resonant gyroscope is actively excited. When the amplitude of the output signal of the hemispherical resonant gyroscope is greater than the set threshold value, the excitation is stopped, and S1 and S3 are executed in sequence; S3: averaging two adjacent identified frequency values, and using the average value as the frequency of the hemispherical resonant gyroscope.

3. The fully digital autonomous frequency control method of a hemispherical resonant gyroscope according to claim 2, characterized in that: If the judgment fails, the hemispherical resonant gyroscope is actively excited. The method of active excitation is: The hemispherical resonant gyroscope is excited by a variable frequency sinusoidal signal. The frequency of the sinusoidal signal changes from an initial value to Gradually increase to the final frequency value ; An active excitation circuit is constructed, including an ADC circuit, a DAC circuit and a digital processing unit. The ADC circuit samples the output signal of the hemispherical resonant gyroscope, and the sampled digital signal is used as the input of the digital processing unit. The output of the digital processing unit is converted by the DAC circuit and then input into the hemispherical resonant gyroscope to form a ring loop; the digital processing unit is used to set the phase and gain. By setting the gain and phase, the loop gain is not less than 1, and the phase satisfies , is a non-negative integer, making the loop a positive feedback loop; when the amplitude of the hemispherical resonant gyroscope output signal sampled by the ADC current is greater than the set threshold value, the excitation is stopped.

4. The fully digital autonomous frequency control method of a hemispherical resonant gyroscope according to claim 3, characterized in that: For a sinusoidal signal with a variable frequency, the step frequency of the frequency change is determined by the quality factor of the hemispherical resonant gyroscope, and the step time is determined by the resonant frequency of the hemispherical resonant gyroscope.

5. The fully digital autonomous frequency control method of a hemispherical resonant gyroscope according to claim 1, characterized in that: Based on the mathematical model of frequency and phase discrimination, the output signal of the hemispherical resonant gyroscope is subjected to frequency and phase discrimination processing; the mathematical model is: In the formula, represents the state observation; It represents the reference signal used for frequency and phase discrimination. The initial state is the reference signal generated by the identified hemispherical resonant gyro frequency. represents white noise, represents the observed white noise; represents the amplitude of the HRG output signal relative to the reference signal, It represents the phase difference of the HRG signal relative to the reference signal.

6. The fully digital autonomous frequency control method of a hemispherical resonant gyroscope according to claim 5, characterized in that: Based on the mathematical model, the phase difference of the hemispherical resonant gyroscope output signal relative to the reference signal is obtained by recursive calculation. , specifically: In the formula, express time The estimated value of express Always exist The estimated value of the moment, , for The intermediate variable at the moment, express Always exist The estimated value of the moment, , for and The covariance matrix of .

7. The fully digital autonomous frequency control method of a hemispherical resonant gyroscope according to claim 1, characterized in that: The phase difference signal is digitally loop filtered, which is expressed as: In the formula, is the gain parameter, is the integration parameter, for The phase difference signal of the hemispherical resonant gyro signal relative to the reference signal at time instant, for The output signal obtained by digital loop filtering at this moment.

8. The fully digital autonomous frequency control method of a hemispherical resonant gyroscope according to claim 1, characterized in that: The frequency of the hemispherical resonant gyro is used as the initial frequency of the digital controlled oscillator, the voltage signal is used as the input, and the frequency signal is output. for: In the formula, is the filtered voltage signal, Represents the number of binary bits required to implement a numerically controlled oscillator, is the base frequency of the digital controlled oscillator, is the initial frequency of the numerically controlled oscillator.

9. The fully digital autonomous frequency control method of a hemispherical resonant gyroscope according to claim 1, characterized in that: The digital reference signal source is implemented by using a CORDIC algorithm to generate a waveform signal of the output frequency signal of the digital controlled oscillator as a reference signal.

Images