Gyroscope real-time self-calibration system and method

Abstract

The gyroscope real-time self-calibration system and method of the present invention mainly calculates the output of the gyroscope after correction by calibration coefficients K and B at certain time intervals when the gyroscope is in a stationary state (zero input), sets an array of a certain length for storing the output of the gyroscope after correction, and the array values ​​can be filled in sequence or updated cyclically. The mean value of the array data is used as an estimated value of the random error of the gyroscope, and is subtracted in real time to obtain a higher-precision attitude angle.

Description

Technical Field

[0001] The present application relates to the field of sensor technology, and in particular to artificial intelligence inertial navigation systems such as those used in intelligent robots, intelligent sweepers, drone attitude reference systems, robot real-time positioning and navigation systems, unmanned driving systems, and vehicle-mounted navigation systems. Background Art

[0002] A gyroscope is a sensor device used to measure the rotation angle or angular displacement of a moving carrier relative to inertial space. It is used to measure and control the attitude and trajectory of the moving carrier. It is an inertial sensor element widely used in modern aviation, navigation, aerospace and defense industries, and is an important part of the Inertial Navigation System (INS).

[0003] In the field of precision agriculture, gyroscopes are often used in machine positioning and navigation applications including automatic harvesters and automatic pesticide sprayers; in the field of smart homes, the applications of gyroscopes mainly include smart sweeping machines and service robots; in the field of smart industry and smart logistics, gyroscopes are mainly used in attitude reference systems and UAV inertial navigation, including UAV attitude reference systems, aerial photography stabilization systems and UAV express delivery systems; in the field of Internet of Vehicles, gyroscopes are often used in vehicle and freight intelligent management systems, for example, they can monitor vehicle location and driving trajectory, and can also manage vehicle posture and call for help; in the field of smart medical care, common applications of gyroscopes include gesture recognition systems for the deaf and mute and bionic prostheses.

[0004] With the rapid development of modern microelectronics and micromachining technology, MEMS gyroscopes based on MicroElectromechanical System (MEMS) technology have developed rapidly. MEMS gyroscopes have the advantages of low cost, small size, low power consumption, light weight and high reliability, and have broad application prospects in the fields of drones, precision-guided missiles, etc.

[0005] At present, compared with fiber optic gyroscopes and laser gyroscopes, MEMS gyroscopes have lower accuracy. Due to the influence of sensor noise, drift and nonlinearity of scale factors, the gyroscope output will produce large errors, and the accuracy of MEMS gyroscopes will decrease with time. These errors will serve as error sources for inertial navigation solution in the strapdown inertial navigation system, affecting the accuracy of the aircraft's attitude solution, thereby causing large heading errors.

[0006] The errors that affect the accuracy of MEMS gyroscopes mainly include deterministic errors and random errors. Deterministic errors are systematic errors that can be eliminated through calibration and other links, while random errors have no obvious rules and are the main bottleneck limiting the improvement of MEMS gyroscope accuracy. These errors will eventually be fed back to the attitude angle update. The deterministic errors of MEMS gyroscopes (including scale factor errors and axis misalignment errors) are generally calculated using algebraic compensation methods. By establishing an accurate error compensation model through experimental testing and calibration, the influence of deterministic errors on the overall system can be eliminated. The random errors of MEMS gyroscopes include three types of errors: random drift, trend drift, and temperature drift. Due to the complexity and uncertainty of the generation mechanism, random drift errors are one of the hot spots and difficulties in the field of inertial navigation at home and abroad. Reducing the random drift error of MEMS gyroscopes is an important part of improving their measurement accuracy, and the identification and compensation of random errors of MEMS gyroscopes is an important way to improve gyroscope accuracy. Summary of the invention

[0007] The present invention proposes an algorithm, which is mainly used to compensate for the random error of the MEMS gyroscope, thereby improving the stability and accuracy of attitude angle update calculation.

[0008] The randomness of the dynamic and static state errors output by the gyro system is an objective existence and it is impossible to completely eliminate it. In probability theory and statistics, the mathematical expectation (mean) (or mean, also referred to as expectation) is the sum of the probability of each possible result in the experiment multiplied by its result, which is one of the most basic mathematical characteristics. It reflects the size of the average value of the random variable. The peak-to-peak value refers to the maximum and minimum values ​​that appear in a certain interval, reflecting the range of values ​​of the random variable. The present invention proposes a method that can improve the accuracy of dynamic and static state recognition and judgment.

[0009] The present invention mainly calculates the output of the gyroscope after correction by calibration coefficients K and B at certain time intervals when the gyroscope is in a stationary state (zero input), sets an array of a certain length for storing the output of the gyroscope after correction, and the array values ​​can be filled in sequence or updated cyclically. The mean value of the array data is used as an estimated value of the random error of the gyroscope, and is subtracted in real time to obtain a higher-precision attitude angle. BRIEF DESCRIPTION OF THE DRAWINGS

[0010] Figure 1 Gyroscope self-calibration parameter calculation and acquisition system,

[0011] Figure 2 Gyroscope random error compensation system and its processing flow. Implementation

[0012] Important abbreviations used in this invention

[0013] GYRO:The abbreviation of Gyroscope, which means gyroscope;

[0014] MEMS: Micro-Electro-Mechanical System;

[0015] ROLL: Roll angle, that is, the angle of rotation around the Y axis;

[0016] PITCH: pitch angle, that is, the angle of rotation around the X axis;

[0017] YAW: yaw angle, that is, the angle of rotation around the Z axis;

[0018] AutoCalibration: automatic calibration, i.e. real-time self-calibration of the gyroscope;

[0019] Calibration: Calibration is the process of calibrating the gyroscope through calibration coefficients.

[0020] Since the gyroscope has random errors, the output of the gyroscope is not zero even when the gyroscope is stationary (zero input). This non-zero sequence can be considered a time series, and the autoregressive AR(n) model is used here.

[0021] This model represents the observation at time t as a linear regression combination of the observations at the past n times:

[0022]

[0023] In formula (5-1) is the autoregressive coefficient, x t is the observed value at the current moment, x t-1 、x t-2 , ..., x t-n are the observation values ​​at the past n moments respectively.

[0024] The random process described by the AR(n) model is the observation x t and the n observations x before time t t-1 、x t-2 , …, x t-n There is a correlation.

[0025] The real-time self-calibration method of gyroscope mainly includes two parts: one is the calculation of gyroscope self-calibration parameter GyroBias, that is, the calculation of gyroscope random error; the other is the compensation of gyroscope random error, that is, compensating the random error into the real-time system to improve the attitude solution accuracy of the whole system.

[0026] Figure 1A gyroscope self-calibration parameter calculation and acquisition system is shown, which comprises a gyroscope (1), a system calibration module (2), and a self-calibration parameter calculation module (3), wherein the gyroscope is used to output an original angular velocity, the system calibration module is used to collect the original angular velocity output by the gyroscope for system calibration, that is, to correct the original angular velocity data through calibration coefficients K and B, and the self-calibration parameter calculation module is used to calculate the original angular velocity after system calibration to obtain self-calibration parameters. The specific implementation process is as follows:

[0027] 1. Set the gyroscope to a stationary state. When the gyroscope is in a stationary state, it is in a zero input state. In the stationary state, the gyroscope outputs angular rate data, which is recorded as GYRO.Raw.

[0028] 2. The system calibration module collects the angular rate data output by the gyroscope, GYRO.Raw(m), m = 1, 2, 3..., m is the sampling point. The collected angular rate is system-calibrated through the calibration coefficients K and B to obtain the angular velocity GYRO.Cal after system calibration, GYRO.Cal(m) = GYRO.Raw(m)*K+B.

[0029] 3. At a fixed time interval T, calculate the mean of the GYRO.Cal sampling points in the continuous time T. This mean can be used as an estimate of the gyroscope random error in the time period T. Select n consecutive time intervals T in the past, denoted as T1, T2, ..., T i ,…,T n ; The mean value of GYRO.Cal in the T1 time interval, denoted as GYRO.Cal_T t-1 , which is the estimated value of the random error of the gyroscope in the T1 time interval, T i The average value of GYRO.Cal over time is denoted as GYRO.Cal_T t-i , which is T i An estimate of the random error of the gyroscope during the time interval, T n The mean value of GYRO.Cal in the time interval, denoted as GYRO.Cal_T t-n , which is T n An estimate of the random error of the gyroscope over the time interval.

[0030] 4. Set up an n-dimensional array, called BiasList, to store GYRO.Cal_T t-1 、GYRO.Cal_T t-2 , …, GYRO.Cal_T t-i , …, GYRO.Cal_T t-n .BiasList = [GYRO.Cal_T t-1 ,GYRO.Cal_Tt-2 , …, GYRO.Cal_T t-i , …GYRO.Cal_T t-n ]. BiasList can be designed as a sliding window, and the BiasList value will be updated in real time, that is, the random error GyroBias_T of the gyroscope at the corresponding moment. t When updated, the estimated value of the gyroscope random error can be dynamically calculated in real time, and the BiasList array can be updated in real time.

[0031] 5. According to formula (5-1), calculate the random error observation value of the gyroscope at the current moment. The random error observation value of the gyroscope at the current moment is a linear combination of the past n random error estimates, and the autoregressive coefficient is If both are taken as 1 / n, then the current gyroscope random error observation value GyroBias_T t is the mean of the past n random error estimates, that is, the mean of all elements in BiasList, As the BiasList array is updated in real time, GyroBias_T t The estimated value of is also calculated and updated in real time, so that the random error of the gyroscope can be calculated dynamically in real time. We estimate the random error of the gyroscope based on the output of the gyroscope when it is in a stationary state. In the subsequent process, the random error will be used to compensate the angular rate of the gyroscope in real time.

[0032] 6. When the gyroscope is detected to be started, the random error observation value of the gyroscope stops updating in real time, and the random error observation value of the gyroscope remains the random error observation value at the last moment before the gyroscope is started.

[0033] Figure 2 The invention relates to a gyroscope random error compensation system and its processing flow. The gyroscope random error compensation system comprises a gyroscope (1), a system calibration module (2), a self-calibration parameter calculation module (3), a self-calibration module (4) and an attitude solution module (5), wherein the gyroscope is used to output the original angular velocity, the system calibration module is used to collect the original angular velocity output by the gyroscope for system calibration, that is, to correct the original angular velocity data by calibration coefficients K and B, the self-calibration parameter calculation module is used to calculate the original angular velocity after system calibration to obtain the self-calibration parameters, the self-calibration module is used to calibrate the original angular velocity after system calibration output by the system calibration module according to the self-calibration parameters output by the self-calibration parameter calculation module, and the calibrated data can be used for subsequent attitude solution. The specific implementation process is as follows:

[0034] 1. The gyroscope outputs angular rate data in real time, and the angular rate is recorded as GYRO.Raw.

[0035] 2. The system calibration module collects the angular rate data output by the gyroscope, GYRO.Raw(m), m = 1, 2, 3..., m is the sampling point. The collected angular rate is system-calibrated by calibration coefficients K and B to obtain the angular rate GYRO.Cal after system calibration, GYRO.Cal(m) = GYRO.Raw(m)*K+B. Two angular rate data GYRO.Cal(m) are output and sent to the self-calibration parameter calculation module and the self-calibration module respectively.

[0036] 3. The self-calibration parameter calculation module detects the dynamic and static state of the gyroscope in real time. When the gyroscope is detected to be in a static state, the mean value of the GYRO.Cal sampling points in the continuous time T is calculated at a fixed time interval T. The mean value can be used as the estimated value of the random error of the gyroscope in the time period T. Select n consecutive time intervals T in the past, respectively recorded as T1, T2, ..., T i ,…,T n ; The mean value of GYRO.Cal in the T1 time interval, denoted as GYRO.Cal_T t-1 , which is the estimated value of the random error of the gyroscope in the T1 time interval, T i The average value of GYRO.Cal over time is denoted as GYRO.Cal_T t-i , which is T i An estimate of the random error of the gyroscope during the time interval, T n The mean value of GYRO.Cal in the time interval, denoted as GYRO.Cal_T t-n , which is T n The estimated value of the random error of the gyroscope during the time interval. Set up an n-dimensional array, called BiasList, to store GYRO.Cal_T t-1 、GYRO.Cal_T t-2 , …, GYRO.Cal_T t-i , …, GYRO.Cal_T t-n .BiasList = [GYRO.Cal_T t-1 ,GYRO.Cal_T t-2 , …, GYRO.Cal_T t-i , …GYRO.Cal_T t-n ]. BiasList can be designed as a sliding window, and the BiasList value will be updated in real time, that is, the random error GyroBias_T of the gyroscope at the corresponding moment. tThe estimated value of the gyroscope random error can be dynamically calculated in real time, and the BiasList array can be updated in real time. According to formula (5-1), the observed value of the gyroscope random error at the current moment is calculated. The observed value of the gyroscope random error at the current moment is a linear combination of the past n random error estimates, and the autoregressive coefficient If both are taken as 1 / n, then the current gyroscope random error observation value GyroBias_T t is the mean of the past n random error estimates, that is, the mean of all elements in BiasList, As the BiasList array is updated in real time, GyroBias_T t The estimated value of is also calculated and updated in real time, so that the random error of the gyroscope can be calculated dynamically in real time. We estimate the random error of the gyroscope based on the output of the gyroscope when it is in a stationary state. When the gyroscope is detected to be in a stationary state, the random error observation value will be updated in real time. When the gyroscope is detected to be in a dynamic state, the random error observation value of the gyroscope stops updating in real time and maintains the random error observation value at the most recent moment unchanged.

[0037] 4. The self-calibration module receives the gyroscope random error observation value output by the self-calibration parameter calculation module and the angular velocity data GYRO.Cal(m) output by the system calibration module in real time, and calibrates the angular velocity data GYRO.Cal(m) according to the gyroscope random error observation value. When the gyroscope is in dynamic or static state, it goes through the self-calibration link, and the random error GyroBias_T output by the self-calibration parameter calculation module is subtracted in real time. t , to compensate for the random error of the gyroscope, the output of the self-calibration module is recorded as GYRO.Data,

[0038] GYRO.Data=GYRO.Cal(m)-GYROBias.T t .

[0039] 5. The attitude angle calculation module is used to receive the calibrated gyroscope angular rate data GYRO.Data output by the self-calibration module, and calculate each attitude angle according to the calibrated gyroscope angular rate, where the attitude angles include roll angle (ROLL), pitch angle (PITCH) and yaw angle (YAW).

[0040] Those of ordinary skill in the art will appreciate that the modules and method steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of the present invention.

[0041] In the embodiments provided in this application, it should be understood that the disclosed devices and methods can be implemented in other ways. For example, the device embodiments described above are merely illustrative.

[0042] In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit.

[0043] If the functions are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, or the part that contributes to the prior art or the part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium, including several instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the methods described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk, etc., which can store program codes.

[0044] The above description is only a specific implementation mode of the present invention, but the protection scope of the present invention is not limited thereto. Any technician familiar with the technical field can easily think of changes or substitutions within the technical scope disclosed by the present invention, which should be covered by the protection scope of the present invention.

Claims

1. A MEMS gyroscope self-calibration parameter calculation and acquisition system, comprising a gyroscope (1), a system calibration module (2), and a self-calibration parameter calculation module (3), wherein the gyroscope is used to output an original angular rate, the system calibration module is used to collect the original angular rate output by the gyroscope for system calibration, and the self-calibration parameter calculation module is used to calculate the original angular rate after system calibration to obtain self-calibration parameters, which is specifically implemented as follows: (1) Set the gyroscope to a stationary state. When the gyroscope is in a stationary state, it is in a zero input state. In the stationary state, the gyroscope outputs angular rate data, which is recorded as GYRO.Raw. (2) The system calibration module collects the angular rate data GYRO.Raw(m) output by the gyroscope, where m = 1, 2, 3, ..., m is the sampling point, and performs system correction on the collected angular rate through calibration coefficients K and B to obtain the angular velocity GYRO.Cal(m) after system correction, GYRO.Cal(m) = GYRO.Raw(m)*K+B; (3) The self-calibration parameter calculation module calculates the mean value of the GYRO.Cal sampling points within a continuous time T at a fixed time interval T. The mean value can be used as the estimated value of the gyroscope random error within the time period T. Select n consecutive time intervals T in the past and record them as T1, T2, ..., T i ,…,T n , where the mean value of GYRO.Cal in the T1 time interval is denoted as GYRO.Cal_T t-1 , which is the estimated value of the random error of the gyroscope in the T1 time interval, T i The average value of GYRO.Cal over time is denoted as GYRO.Cal_T t-i , which is T i An estimate of the random error of the gyroscope during the time interval, T n The mean value of GYRO.Cal in the time interval, denoted as GYRO.Cal_T t-n , which is T n The estimated value of the random error of the gyroscope during the time interval; set up an n-dimensional array, denoted as BiasList, to save GYRO.Cal_T t-1 、GYRO.Cal_T t-2 , …, GYRO.Cal_T t-i , …, GYRO.Cal_T t-n , BiasList = [GYRO.Cal_T t-1 ,GYRO.Cal_T t-2 , …, GYRO.Cal_T t-i , …GYRO.Cal_T t-n], where BiasList is designed as a sliding window, the BiasList value is updated in real time, and the BiasList array is updated in real time; (4) The self-calibration parameter calculation module further calculates the random error observation value GyroBias_Tt of the gyroscope at the current moment, GyroBias_Tt is the self-calibration parameter of the self-calibration gyroscope, which is used to calculate the self-calibration parameters output by the system; (5) When the self-calibration parameter calculation module detects that the gyroscope is started, the random error observation value of the gyroscope stops updating in real time, and the random error observation value of the gyroscope is maintained as the random error observation value at the last moment before the gyroscope is started.

2. A method for calculating and acquiring self-calibration parameters of a MEMS gyroscope, which is applied to a gyroscope self-calibration parameter calculation and acquisition system, the system comprising a gyroscope (1), a system calibration module (2), and a self-calibration parameter calculation module (3), wherein the gyroscope is used to output an original angular velocity, the system calibration module is used to collect the original angular velocity output by the gyroscope for system calibration, and the self-calibration parameter calculation module is used to calculate the original angular velocity after system calibration to obtain self-calibration parameters, and the specific steps are: (1) Set the gyroscope to a stationary state. When the gyroscope is in a stationary state, it is in a zero input state. In the stationary state, the gyroscope outputs angular rate data, which is recorded as GYRO.Raw. (2) The system calibration module collects the angular rate data GYRO.Raw(m) output by the gyroscope, where m = 1, 2, 3, ..., m is the sampling point, and performs system correction on the collected angular rate through calibration coefficients K and B to obtain the angular velocity GYRO.Cal(m) after system correction, GYRO.Cal(m) = GYRO.Raw(m)*K+B; (3) The self-calibration parameter calculation module calculates the mean value of the GYRO.Cal sampling points within a continuous time T at a fixed time interval T. The mean value can be used as the estimated value of the gyroscope random error within the time period T. Select n consecutive time intervals T in the past and record them as T1, T2, ..., T i ,…,T n , where the mean value of GYRO.Cal in the T1 time interval is denoted as GYRO.Cal_T t-1 , which is the estimated value of the random error of the gyroscope in the T1 time interval, T i The average value of GYRO.Cal over time is denoted as GYRO.Cal_T t-i , which is T i An estimate of the random error of the gyroscope during the time interval, T n The mean value of GYRO.Cal in the time interval, denoted as GYRO.Cal_T t-n, which is T n The estimated value of the random error of the gyroscope during the time interval; set up an n-dimensional array, denoted as BiasList, to save GYRO.Cal_T t-1 、GYRO.Cal_T t-2 , …, GYRO.Cal_T t-i , …, GYRO.Cal_T t-n , BiasList = [GYRO.Cal_T t-1 ,GYRO.Cal_T t-2 , …, GYRO.Cal_T t-i , …GYRO.Cal_T t-n ], where BiasList is designed as a sliding window, the BiasList value is updated in real time, and the BiasList array is updated in real time; (4) The self-calibration parameter calculation module further calculates the random error observation value GyroBias_T of the gyroscope at the current moment t , GyroBias_T t That is, the self-calibration parameters of the self-calibration gyroscope are calculated to obtain the self-calibration parameters output by the system; (5) When the self-calibration parameter calculation module detects that the gyroscope is started, the random error observation value of the gyroscope stops updating in real time, and the random error observation value of the gyroscope is maintained as the random error observation value at the last moment before the gyroscope is started.

3. A MEMS gyroscope random error compensation system, comprising a gyroscope (1), a system calibration module (2), a self-calibration parameter calculation module (3), a self-calibration module (4) and an attitude angle solution module (5), wherein the gyroscope is used to output an original angular rate, the system calibration module is used to collect the original angular rate output by the gyroscope for system calibration, the self-calibration parameter calculation module is used to calculate the original angular rate after system calibration to obtain self-calibration parameters, the self-calibration module is used to calibrate the gyroscope angular rate output by the system calibration module according to the self-calibration parameters output by the self-calibration parameter calculation module, and the calibrated data is input into the attitude angle solution module, and is specifically implemented as follows: (1) The gyroscope outputs angular rate data in real time, and the angular rate is recorded as GYRO.Raw; (2) The system calibration module collects the angular rate data output by the gyroscope, GYRO.Raw(m), m = 1, 2, 3..., m is the sampling point, and performs system correction on the collected angular rate through calibration coefficients K and B to obtain the angular rate after system correction GYRO.Cal, GYRO.Cal(m) = GYRO.Raw(m)*K+B, and outputs two angular rate data GYRO.Cal(m), which are sent to the self-calibration parameter calculation module and the self-calibration module respectively; (3) The self-calibration parameter calculation module detects the dynamic and static state of the gyroscope in real time. When the gyroscope is detected to be in a static state, the mean value of the GYRO.Cal sampling points within a continuous time T is calculated at a fixed time interval T. The past n consecutive time intervals T are selected and recorded as T1, T2, ..., T i ,…,T n , the mean value of GYRO.Cal in the T1 time interval, denoted as GYRO.Cal_T t-1 , which is the estimated value of the random error of the gyroscope in the T1 time interval, T i The average value of GYRO.Cal over time is denoted as GYRO.Cal_T t-i , which is T i An estimate of the random error of the gyroscope during the time interval, T n The mean value of GYRO.Cal in the time interval, denoted as GYRO.Cal_T t-n , which is T n The estimated value of the random error of the gyroscope during the time interval; set up an n-dimensional array, denoted as BiasList, to save GYRO.Cal_T t-1 、GYRO.Cal_T t-2 , …, GYRO.Cal_T t-i , …, GYRO.Cal_T t-n , BiasList = [GYRO.Cal_T t-1 ,GYRO.Cal_T t-2 , …, GYRO.Cal_T t-i , …GYRO.Cal_T t-n ], BiasList can be designed as a sliding window, the BiasList value is updated in real time, and the BiasList array is updated in real time; The current gyroscope random error observation value GyroBias_T t is the mean of the past n random error estimates, that is, the mean of all elements in the BiasList array, When the gyroscope is detected to be in a dynamic state, the random error observation value GyroBias_T of the gyroscope t Stop real-time updating and output the GyroBias_Tt value at the last moment; (4) The self-calibration module receives the gyroscope random error observation value output by the self-calibration parameter calculation module and the angular velocity data GYRO.Cal(m) output by the system calibration module in real time, calibrates the angular velocity data GYRO.Cal(m) according to the gyroscope random error observation value, and outputs the gyroscope angular velocity GYRO.Data after self-calibration, GYRO.Data=GYRO.Cal(m)-GYROBias.T t ; (5) The attitude angle calculation module is used to receive the calibrated gyroscope angular rate data GYRO.Data output by the calibration module, and calculate each attitude angle according to the calibrated gyroscope angular rate.

4. A method for compensating a random error of a MEMS gyroscope, applied to a random error compensation system of a gyroscope, the system comprising a gyroscope (1), a system calibration module (2), a self-calibration parameter calculation module (3), a self-calibration module (4) and an attitude angle solution module (5), wherein the gyroscope is used to output an original angular velocity, the system calibration module is used to collect the original angular velocity output by the gyroscope for system calibration, the self-calibration parameter calculation module is used to calculate the original angular velocity after system calibration to obtain self-calibration parameters, the self-calibration module is used to calibrate the gyroscope angular velocity output by the system calibration module according to the self-calibration parameters output by the self-calibration parameter calculation module, and the calibrated data is input into the attitude angle solution module, and the specific steps are as follows: (1) The gyroscope outputs angular rate data in real time, and the angular rate is recorded as GYRO.Raw; (2) The system calibration module collects the angular rate data output by the gyroscope, GYRO.Raw(m), m = 1, 2, 3..., m is the sampling point, and performs system correction on the collected angular rate through calibration coefficients K and B to obtain the angular rate after system correction GYRO.Cal, GYRO.Cal(m) = GYRO.Raw(m)*K+B, and outputs two angular rate data GYRO.Cal(m), which are sent to the self-calibration parameter calculation module and the self-calibration module respectively; (3) The self-calibration parameter calculation module detects the dynamic and static state of the gyroscope in real time. When the gyroscope is detected to be in a static state, the mean value of the GYRO.Cal sampling points within a continuous time T is calculated at a fixed time interval T. The past n consecutive time intervals T are selected and recorded as T1, T2, ..., T i ,…,T n , the mean value of GYRO.Cal in the T1 time interval, denoted as GYRO.Cal_T t-1 , which is the estimated value of the random error of the gyroscope in the T1 time interval, T i The average value of GYRO.Cal over time is denoted as GYRO.Cal_T t-i , which is T i An estimate of the random error of the gyroscope during the time interval, T n The mean value of GYRO.Cal in the time interval, denoted as GYRO.Cal_T t-n , which is T n The estimated value of the random error of the gyroscope during the time interval; set up an n-dimensional array, denoted as BiasList, to save GYRO.Cal_T t-1 、GYRO.Cal_T t-2 , …, GYRO.Cal_T t-i , …, GYRO.Cal_T t-n , BiasList = [GYRO.Cal_T t-1 ,GYRO.Cal_T t-2 , …, GYRO.Cal_T t-i , …GYRO.Cal_T t-n ], BiasList can be designed as a sliding window, the BiasList value is updated in real time, and the BiasList array is updated in real time; The current gyroscope random error observation value GyroBias_T t is the mean of the past n random error estimates, that is, the mean of all elements in the BiasList array, When the gyroscope is detected to be in a dynamic state, the random error observation value GyroBias_T of the gyroscope t Stop real-time update and output GyroBias_T at the last moment t value; (4) The self-calibration module receives the gyroscope random error observation value output by the self-calibration parameter calculation module and the angular velocity data GYRO.Cal(m) output by the system calibration module in real time, calibrates the angular velocity data GYRO.Cal(m) according to the gyroscope random error observation value, and outputs the gyroscope angular velocity GYRO.Data after self-calibration, GYRO.Data=GYRO.Cal(m)-GYROBias_T t ; (5) The attitude angle calculation module is used to receive the calibrated gyroscope angular rate data GYRO.Data output by the calibration module, and calculate each attitude angle according to the calibrated gyroscope angular rate.

Images