A method for solving attitude based on angular acceleration extended state quantity

Abstract

The application provides a kind of attitude solution method based on angular acceleration extended state quantity, improve the attitude solution precision of inertial navigation system under the premise of comprehensive consideration cost;The attitude solution system of the application is composed of three-axis liquid ring angular acceleration sensor and low-cost MEMS gyro, the state vector is extended, and modeling is carried out based on the extended state vector, the carrier attitude is solved by adopting Kalman filtering method, fusing angular acceleration sensor data through gyro output;The application adds angular acceleration state quantity in state vector, realizes the close combination of angular acceleration sensor and gyro, and is a kind of positioning method with high precision;Compared with high-precision gyro, the angular acceleration sensor and MEMS gyro used in the application are lower in cost, so the cost can be guaranteed to be low.

Description

Technical Field

[0001] This invention belongs to the field of automatic control technology, specifically relating to an attitude calculation method based on angular acceleration extended state variables. Background Technology

[0002] An inertial navigation system is an autonomous navigation system that does not rely on external information or radiate energy to the outside. Therefore, it has strong autonomy and stealth, and provides complete output results. It has wide applications in various industries such as aerospace, navigation, and autonomous driving.

[0003] Traditional inertial navigation and positioning mainly uses an inertial navigation system composed of linear accelerometers and gyroscopes. It calculates the attitude, orientation and other information of the moving vehicle by measuring data through a combination of sensors. However, because the error accumulates over time during the calculation process, it is difficult to meet the high-precision navigation requirements under long-term conditions.

[0004] Liquid mass angular acceleration sensors have the advantages of high bandwidth, large measurement range, and high accuracy. Using liquid mass angular acceleration sensors in combination with gyroscopes for attitude calculation can process the directly acquired angular acceleration information, thereby improving the calculation accuracy and reducing cumulative errors. In addition, compared with high-precision gyroscopes, angular acceleration sensors are less expensive, which can reduce the cost of the calculation system while ensuring accuracy. Summary of the Invention

[0005] In view of this, the purpose of this invention is to provide an attitude calculation method based on angular acceleration extended state variables, which improves the attitude calculation accuracy of inertial navigation systems while taking cost into consideration.

[0006] An attitude calculation method based on angular acceleration extended state variables includes the following steps:

[0007] Step 1: Periodically read the sensor information of the carrier at time k, including the angular acceleration a(k) of the triaxial angular accelerometer and the angular velocity ω of the triaxial gyroscope. gyro (k);

[0008] Step 2: Noise reduction is performed on the angular accelerometer and gyroscope data;

[0009] Step 3: Integrate the angular acceleration to obtain the angular velocity ω(k);

[0010] Step 4: Kalman filtering to estimate attitude angles, including:

[0011] Step 4(a) Estimate the state vector at time k. The state vector is:

[0012] X(k)=[a(k) ω(k) q(k)] T

[0013] Where a(k) is the angular acceleration obtained in step 1, ω(k) = [ω x (k) ω y (k) ω z [q(k)] represents the integral angular velocity obtained in step 3, and q(k) = [q1(k) q2(k) q3(k) q4(k)] represents the quaternion of the carrier attitude at time k to be solved, which is predicted according to the following formula:

[0014] q(k)=Λ(k)·q(k-1)

[0015] Where q(k-1) is the quaternion of the carrier attitude at time k-1;

[0016]

[0017] In the formula, T is the sampling time;

[0018] Step 4(b): Based on the extended state vector, the Kalman filter method is used to predict the carrier attitude at time k.

[0019] The discrete state model is as follows:

[0020]

[0021] Where, n a (k-1) represents the Gaussian white noise of the angular acceleration at time k-1, and E(k-1) is the noise coefficient matrix at time k-1, specifically:

[0022]

[0023] In the formula, [q1(k-1)q2(k-1)q3(k-1)q4(k-1)] is the quaternion of the carrier attitude at time k-1;

[0024] Step 4(c), the measurement model is:

[0025] Z(k) = [0 3×3 I 3×3 0 3×4 X(k)

[0026] Using gyroscope output ω gyro Z(k) is used as the observation. The state vector at time k is estimated by using the Kalman filter method, so as to obtain the attitude angle quaternion q(k) at time k, and realize the attitude information solution by fusing the data of the angular acceleration sensor and the gyroscope.

[0027] Repeating steps 1 to 4 will yield continuous carrier attitude angle calculation results.

[0028] Preferably, in step 2, after removing outliers according to the 3σ principle, sliding window filtering and Butterworth low-pass filtering methods are used to reduce noise in the angular acceleration sensor and gyroscope data.

[0029] Preferably, in step 3, the composite Simpson method is used to integrate the angular acceleration.

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

[0031] This invention designs an attitude calculation system consisting of a triaxial liquid-ring angular accelerometer sensor and a low-cost MEMS gyroscope. The state vector is extended, and modeling is performed based on the extended state vector. A Kalman filter method is used, and the attitude of the carrier is calculated by fusing angular accelerometer sensor data with gyroscope output. This invention adds an angular acceleration state variable to the state vector, achieving a tight combination of the angular accelerometer sensor and gyroscope, resulting in a high-precision positioning method. Compared to high-precision gyroscopes, the angular accelerometer sensor and MEMS gyroscope used in this invention are less expensive, thus ensuring low cost. Attached Figure Description

[0032] Figure 1 This is a flowchart of the inertial navigation attitude calculation method of the present invention.

[0033] Figure 2 The results are the attitude calculation results of the method of the present invention and the attitude calculation results using only the gyroscope calculation method.

[0034] Figure 3 The results show the attitude calculation error of the method of the present invention and the attitude calculation error of the method using only gyroscope. Detailed Implementation

[0035] The present invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0036] To address the problems of high cost and large cumulative error in existing inertial navigation systems, this invention designs an attitude calculation system and method consisting of a triaxial liquid ring angular acceleration sensor and a low-cost MEMS gyroscope. The specific process is as follows: Figure 1 As shown, it includes:

[0037] Step 1: Sensor Information Reading. Periodically read the sensor information of the carrier at time k, including the angular acceleration a(k) of the triaxial angular accelerometer = [a...]. x (k) a y (k) a z [(k)], angular velocity of a three-axis gyroscope

[0038] Step 2: Sensor data preprocessing. After removing outliers according to the 3σ principle, sliding window filtering and Butterworth low-pass filtering are used to reduce noise in the angular accelerometer and gyroscope data to suppress high-frequency noise and periodic interference.

[0039] Step 3: Integrate angular acceleration. The angular velocity is obtained by integrating the angular acceleration using the combined Simpson integration method, as shown in the following formula:

[0040]

[0041] Where, ω(k)=[ω x (k) ω y (k) ω z [(k)] represents the angular velocity obtained by integrating the angular acceleration at time k, a(k) represents the angular acceleration obtained in step 1 at time k, T represents the sampling time, and n represents the number of equal divisions of the integration interval. Let x be the step size, and let x be the distribution points satisfying x i = k+ih, (i = 0, 1, ..., n-1).

[0042] Step 4: Use Kalman filtering to estimate the attitude angle.

[0043] Step 4(a), State Vector Design. Estimate the state vector at time k. The state vector is:

[0044] X(k)=[a(k) ω(k) q(k)] T

[0045] Where a(k) is the angular acceleration obtained in step 1, ω(k) is the integrated angular velocity obtained in step 3, and q(k) = [q1(k)q2(k)q3(k)q4(k)] is the quaternion of the carrier attitude, which is predicted according to the following formula:

[0046] q(k)=Λ(k)·q(k-1)

[0047] in,

[0048]

[0049] Step 4(b) State estimation. Based on the extended state vector, the Kalman filter method is used to predict the carrier attitude at time k.

[0050] The measurements from the angular acceleration sensor are modeled as a random walk model. Considering the differential relationship between the angular acceleration and the angular velocity, we have:

[0051]

[0052]

[0053] Where, n a Gaussian white noise with angular acceleration.

[0054] According to the attitude differential equation of the carrier, we have:

[0055]

[0056] Where, n ω The state noise is the angular velocity, and E is the noise coefficient matrix of the angular velocity filter, specifically:

[0057]

[0058] Ω(ω) satisfies:

[0059]

[0060] Among them, [ω × ] is the antisymmetric matrix of ω.

[0061] The above angular acceleration sensor model and attitude differential equation constitute the state model of the algorithm. Discretizing this model yields:

[0062]

[0063] Wherein, the state noise of angular acceleration n ω Satisfying n ω =T·n a Therefore, in the above equation, the noise figure matrix of the discretized angular acceleration is T. 2 E(k-1) and E(k-1) satisfy:

[0064]

[0065] Step 4(c), Measurement Estimation. The measurement model is as follows:

[0066] Z(k) = [0 3×3 I 3×3 0 3×4 X(k)

[0067] Using gyroscope output ω gyro (k) is used as the observation. The state vector at time k is estimated by using the Kalman filter method, so as to obtain the attitude angle quaternion q(k) at time k, and realize the attitude information calculation by fusing the data of the angular acceleration sensor and the gyroscope.

[0068] Repeating steps 1 to 4 will yield continuous carrier attitude angle calculation results.

[0069] This invention was experimentally verified using a three-axis turntable. The turntable is equipped with a low-cost MEMS gyroscope and a three-axis angular acceleration sensor, and is connected to a computer via USB and serial port. The turntable is also equipped with a high-precision INS system containing a high-precision laser gyroscope to provide true attitude angle values ​​for the experiment. Figure 2 and Figure 3 The attitude angles and their errors are shown below, for example, the true value, the value calculated using only MEMS gyroscopes, and the value calculated using the combination of angular acceleration sensor and MEMS gyroscopes in this invention. The errors of each calculation method in multiple experiments are as follows:

[0070]

[0071] The results of the turntable experiment show that the inertial navigation system and attitude calculation method proposed in this invention reduce the average absolute error by 43.48%-55.52% compared with the pure gyroscope calculation method, reduce the cumulative error of attitude calculation over a long period of time, and improve the calculation accuracy.

[0072] In summary, the above are merely preferred embodiments of the present invention and are not intended to limit the scope of protection of the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. An attitude calculation method based on angular acceleration extended state variables, characterized in that, Includes the following steps: Step 1: Periodically read the carrier. Sensor information at any given time, including angular acceleration from a triaxial angular accelerometer. Angular velocity of a three-axis gyroscope ; Step 2: Noise reduction is performed on the angular accelerometer and gyroscope data; Step 3: Integrate the angular acceleration to obtain the angular velocity. ; Step 4: Kalman filtering to estimate attitude angles, including: Step 4(a) Estimation The state vector at time t is: in, The angular acceleration obtained in step 1, The integral angular velocity obtained in step 3 is... For the solution to be found The carrier attitude quaternion at any given time is predicted using the following formula: in, for The carrier attitude quaternion at a given moment; In the formula, Sampling time; Step 4(b): Based on the extended state vector, the Kalman filter method is used to... Predicting the carrier's attitude at any given moment: The discrete state model is as follows: in, for Gaussian white noise of angular acceleration at time step for The time-order noise coefficient matrix is ​​as follows: In the formula, for The quaternion of the carrier attitude at a given moment; Step 4(c), the measurement model is: Using the angular velocity of a three-axis gyroscope As an observation Using the Kalman filter method, for State vector at time step To make an estimate, and thus obtain The carrier attitude quaternion of time This enables attitude information calculation by fusing data from angular acceleration sensors and gyroscopes; Repeating steps 1 to 4 will yield continuous carrier attitude angle calculation results.

2. The attitude calculation method based on angular acceleration extended state variables as described in claim 1, characterized in that, In step 2, according to After removing outliers, sliding window filtering and Butterworth low-pass filtering are used to reduce noise in the angular acceleration sensor and gyroscope data.

3. The attitude calculation method based on angular acceleration extended state variables as described in claim 1, characterized in that, In step 3, the composite Simpson method is used to integrate the angular acceleration.

Images