A vibration-resistant attitude and heading reference filter based on inertial measurements

Abstract

The application provides a kind of anti-vibration navigation filter method based on inertial measurement, belong to inertial navigation technical field.The method of the application will be inertial sensor fixed in carrier, based on carrier attitude construction system state variable, based on current gyroscope measurement value and state equation prediction current time step carrier state;According to accelerometer measurement value, the carrier attitude is calculated and is used as observation vector;According to acceleration error, the fluctuation upper limit of attitude angle is calculated, whether the current predicted carrier state is accurate is judged, whether the current reaches filtering period is judged, whether the current acceleration error reaches the requirement is judged, if the current predicted carrier state is accurate and error reaches the requirement, then according to the current average angular velocity of gyroscope weighted calculation observation noise covariance matrix, measurement update and filtering period update are carried out.The method of the application can be applicable to the situation that the carrier has strong vibration, improves the navigation measurement accuracy, and does not need to calibrate the error parameters of gyroscope and acceleration.

Description

Technical Field

[0001] This invention relates to the field of inertial navigation technology, and in particular to an attitude filtering method based solely on inertial measurements in a vibrating environment. Background Technology

[0002] The basic principle of inertial navigation technology is to estimate the target's position, velocity, and attitude in real time by measuring acceleration and angular velocity, combining this with known initial states, and using mathematical models for integral calculations. Inertial navigation systems collect inertial data through their own sensitive devices, requiring no external data input and radiating no energy externally. This gives them advantages such as high stealth, strong anti-interference capabilities, and complete autonomy.

[0003] However, since the positioning solution of inertial navigation systems is obtained by integrating acceleration and angular velocity, its error accumulates and diverges over time and cannot be eliminated. This is especially true for low-cost microelectromechanical system (MEMS) inertial navigation systems, where the measurement accuracy is lower and errors are more likely to accumulate, affecting the long-term stability and accuracy of the system.

[0004] When a vehicle is stationary or moving at a constant velocity relative to an inertial reference frame, its attitude can be calculated using accelerometer measurements. This attitude calculation method is instantaneous, and its errors do not accumulate over time, thus effectively solving the error accumulation problem that occurs when calculating attitude through angular velocity integration in inertial navigation systems.

[0005] However, in practical applications, the errors in the carrier attitude angles calculated using the above method mainly stem from two aspects: first, the accelerometer measurements contain errors; second, in reality, the carrier typically does not remain stationary or move at a uniform linear velocity relative to the inertial frame, thus not strictly meeting the conditions for the above formula to hold. Therefore, the technique of relying solely on inertial measurements to obtain carrier attitude needs further improvement to ensure the accuracy of carrier attitude measurements. Summary of the Invention

[0006] To address the problem of inaccurate carrier attitude angle measurement in practical applications, this invention provides an anti-vibration attitude filtering method based on inertial measurement. The aim is to ensure the attitude accuracy of a low-cost inertial navigation system during long-term independent application and when the carrier experiences strong vibrations, by relying solely on inertial measurement.

[0007] This invention provides a vibration-resistant attitude filtering method based on inertial measurement. The inertial sensor is mounted inside a carrier and is fixedly connected to the carrier. The three sensitive axes of the inertial sensor align with the three axes of the carrier's coordinate system. The method uses a Kalman filter to construct an attitude filtering model, and includes the following steps:

[0008] Step 1: Set the system state variable X as follows:

[0009] X = [X1 X2 X3] T ;

[0010] X1 = [θφψ];

[0011] X2=[b gx b gy b gz ];

[0012] X3 = [S x S y S z ];

[0013] Where θ, φ, and ψ represent the vehicle's pitch angle, roll angle, and yaw angle, respectively; b gx b gy b gz These represent the three-axis components of the gyroscope's zero bias; S x S y S z These represent the three-axis scaling factors of the gyroscope.

[0014] Step 2: Construct the state equations as follows:

[0015]

[0016] in, These are the three-axis measurements from the gyroscope. X1(1) and X1(2) represent the noise of the gyroscope measurement; X1(1) and X1(2) represent the first and second elements of the vector X1, respectively.

[0017] Obtain the current three-axis measurement values ​​of the gyroscope, perform time update steps according to the system state equation, predict the current system state based on the previous system state, and obtain the predicted carrier attitude X1 at the current moment.

[0018] Step 3: Based on the accelerometer measurement data, calculate the carrier attitude as follows:

[0019]

[0020] Where, θ acc φ acc These represent the pitch angle and roll angle calculated from accelerometer measurement data, respectively. These represent the triaxial measurements of the accelerometer; g is the acceleration due to gravity.

[0021] Step 4: Calculate the upper bound of Euler angle fluctuation based on the error Δa, including:

[0022] Calculate the current error Next, calculate the pitch and roll angles of the fluctuation when Δa is distributed to different axes of the accelerometer, and determine the upper bound R of the pitch angle fluctuation. θ The upper bound R of the roll angle fluctuation φ .

[0023] Step 5: Determine whether the predicted carrier attitude at the current moment meets the following conditions:

[0024]

[0025] If the conditions are met, it indicates that the current predicted carrier attitude is accurate, and the error requirement assessment continues; if the conditions are not met, it indicates that the current predicted carrier attitude is inaccurate, and the assessment continues to determine whether the current filtering period T has been reached. S If the threshold is not reached, proceed to step two. If the threshold is reached, perform an error requirement judgment. This error requirement judgment means determining whether the current error Δa is less than the set threshold. If yes, proceed to step six; otherwise, proceed to step two.

[0026] Step 6: Calculate the pitch angle θ from the accelerometer. acc and roll angle φ acc As the observation vector, the system observation equations are constructed as follows:

[0027]

[0028] Where, the observation vector Z = [θ acc φ acc ] T H is the observation matrix, and v is the measurement noise.

[0029] Step 7: Calculate the observation noise covariance matrix R based on the average angular velocity ω, including:

[0030] First, calculate the average angular velocity. Next, calculate the weights. And 0.1 < k ω <10; then calculate the matrix. Where k0 and k1 are both set coefficients.

[0031] The measurement update step of Kalman filtering is performed, which calculates the Kalman gain matrix based on the observation noise covariance matrix R calculated at the current time, and then corrects the predicted system state X at the current time.

[0032] Step 8: Set the current filter period T based on the average angular velocity ω. S as follows:

[0033] Where k2 is a coefficient, T S The unit is seconds.

[0034] Perform steps two through five within the current filtering cycle.

[0035] Compared to existing technologies, the advantages and positive effects of this invention are as follows: The method employs Kalman filtering, using accelerometer measurement data to calculate pitch and roll angles, which are then used as system observations. Attitude is filtered and corrected by appropriately setting the observation noise covariance matrix, thereby improving the system's attitude estimation accuracy. This method can be applied under conditions of strong airframe vibration, without requiring calibration of gyroscope and accelerometer error parameters. It ensures the attitude accuracy of a low-cost inertial navigation system under prolonged independent operation and strong airframe vibration solely through inertial measurements. This method is simple, practical, and significantly improves the accuracy of attitude measurement. Attached Figure Description

[0036] Figure 1 This is a schematic flowchart of the attitude filtering method based on inertial measurement according to the present invention. Detailed Implementation

[0037] The present invention will be further described below with reference to the accompanying drawings and specific embodiments, so that those skilled in the art can better understand and implement the present invention. However, the embodiments described are not intended to limit the present invention.

[0038] In the application scenario of this invention embodiment, the inertial navigation system is installed on a drone, with the installation point located at the center of mass of the drone carrier. The sensitive direction of the inertial sensor is the same as the three axes of the carrier coordinate system. The drone collects data during normal flight, and the attitude of the drone is calculated using the method of this invention. Figure 1 As shown, this invention discloses an attitude filtering method based on inertial measurement. It designs an attitude filtering model based on Euler angles, calculates the pitch and roll angles using accelerometer measurement data, and uses these as system observations. By appropriately setting the observation noise covariance matrix, attitude filtering and correction are achieved. This method can be used under conditions of strong airframe vibration and does not require calibration of the error parameters of the gyroscope and accelerometer. It is simple, practical, and highly effective. The method comprises the following nine steps.

[0039] Step 1: Select and set the system state vector X as follows:

[0040] X = [X1 X2 X3] T ;

[0041] X1 = [θφψ];

[0042] X2=[b gx b gy b gz ];

[0043] X3 = [S x S y S z ];

[0044] Where θ, φ, and ψ represent the vehicle's pitch angle, roll angle, and yaw angle, respectively; b gx b gy b gz These represent the three-axis components of the gyroscope's zero bias; S x S y S z These represent the three-axis scale factors of the gyroscope; [·] T This indicates the matrix transpose.

[0045] Step 2: Construct the state equations as follows:

[0046]

[0047] in, Let F be the time derivative of the state vector X, F be the state transition matrix, G be the input matrix, and w be the process noise vector; matrices These are the three-axis measurements from the gyroscope. X1(1) represents the measurement noise of the three axes of the gyroscope; X1(1) represents the first element of vector X1, which is θ, and the same applies to other cases. 0 represents the zero matrix.

[0048] Based on the relationship between the Euler angle change rate and the gyroscope measurement value, the system state equation is determined in this embodiment of the invention as follows:

[0049]

[0050] Among them, gyroscope measurement values These represent the three-axis measurements of the gyroscope.

[0051] This step predicts the current state based on the previous moment's state and the current gyroscope measurements, i.e., it performs a Kalman filter time update to obtain the predicted vehicle pitch, roll, and yaw angles. The initial state can be directly used from the first observation, or it can be set using prior measurements.

[0052] The time update, or prediction step, of the Kalman filter, including state prediction and covariance prediction, is as follows:

[0053]

[0054] Where k is the filtering time; Based on the state at time k Predict the state at time k+1; Φ(k+1,k) is the state transition matrix from time k to time k+1; P(k+1|k) is the covariance matrix of the predicted state; Γ(k+1,k) is the noise propagation matrix from time k to time k+1, calculated from the state equation; Q k Let be the process noise covariance matrix at time k.

[0055] Step 3: Calculate the carrier attitude based on the accelerometer measurement data.

[0056] When the drone is stationary or moving at a constant velocity in a straight line relative to its inertial frame, the accelerometer output is as follows:

[0057]

[0058] Among them, a x ,a y ,a z These are the ideal measurements of the accelerometer in three axes, respectively; g is the acceleration due to gravity.

[0059] The pitch angle θ can be calculated from the accelerometer output. acc and roll angle φ acc As shown in the following formula:

[0060]

[0061] in These represent the triaxial measurements from the accelerometer.

[0062] Step 4: Calculate the upper bound of Euler angle fluctuation based on the error Δa.

[0063] (1) Calculate the error evaluation index This error assessment index is used to evaluate the difference between a state of motion and a state of rest or uniform linear motion. The larger the calculated error value, the more violent the motion of the carrier. Based on this, this invention calculates the fluctuation of Euler angles and the observation noise covariance matrix R.

[0064] (2) The fluctuation of Euler angles when Δa is distributed along different axes is calculated as follows:

[0065] ① Assigning Δa to the x-axis, the pitch and roll angles of the wave are obtained as follows:

[0066]

[0067] ② Assigning Δa to the z-axis, the pitch and roll angles of the wave are obtained as follows:

[0068]

[0069] ③ Assigning Δa to the y-axis, we obtain the pitch and roll angles of the wave as follows:

[0070]

[0071] (3) Calculate the upper bound R of the fluctuation. θ and R φ as follows:

[0072]

[0073] Step 5: Determine the accuracy of the carrier attitude based on strapdown inertial recursion.

[0074] If the inertial recursive attitudes X1(1) and X1(2) satisfy the following conditions, then the accuracy of the attitude predicted in the current step two is considered to be within an acceptable range.

[0075]

[0076] Based on the above conditions, we can determine whether the attitude angle predicted at the current time step is accurate.

[0077] If the attitude angle meets the above conditions at this time, continue to judge whether the error Δa meets the requirements. If Δa < 0.1m / s 2 This indicates that the requirements are met, and proceed to step six to update the measurement and filter cycle; if Δa ≥ 0.1 m / s 2 If the condition is not met, proceed to step two and wait for IMU input before executing the time update step.

[0078] If the attitude angle does not meet the above conditions at this time, first determine whether the filter period T of the last measurement update has been reached. S If not, proceed to step two, wait for IMU input and execute the time update step. If T is reached... S Then determine whether Δa meets the requirements. If Δa < 0.1 m / s 2 Proceed to step six to update the measurement and filter cycle. If Δa ≥ 0.1 m / s 2 If so, proceed to step two, wait for IMU input, and execute the time update step.

[0079] Step Six: Construct the system observation equations as follows:

[0080]

[0081] Where, the observation vector Z = [θ acc φ acc ] T H is the observation matrix; v is the measurement noise.

[0082] Step 7: Calculate the observation noise covariance matrix R based on the average angular velocity ω.

[0083] (1) Calculate the average angular velocity

[0084] (2) Calculate the weight k ω And set the upper and lower bounds to 0.1 < k ω <10, where k0 and k1 are pre-set coefficients.

[0085]

[0086] (3) The observation noise covariance matrix R is calculated as follows:

[0087]

[0088] The measurement update step of performing Kalman filtering, combined with the observed values ​​to correct the prediction results, is as follows:

[0089]

[0090] Where K(k+1) is the Kalman gain matrix at time k+1; Z(k+1) and H(k+1) are the observation vector and observation matrix at time k+1, respectively, obtained from the observation equation; R k+1 This is the observation noise covariance matrix updated at time k+1; [·] -1 Represents the inverse matrix; P(k+1|k+1) is the corrected optimal system state at time k+1; P(k+1|k+1) is the corrected covariance matrix; I is the identity matrix.

[0091] Step 8: Set the filter period T based on the average angular velocity ω S And set the upper and lower bounds to 0.5 < T S ≤2, where k2 is the coefficient.

[0092]

[0093] Calculated T S The unit is seconds, and the maximum filter period is set to 2 seconds.

[0094] If the average angular velocity ω is small, it indicates that the carrier's rotational motion is not intense, and the accuracy of the accelerometer's attitude measurement is relatively high. In this case, the next measurement update will be performed after a short period of time. If the average angular velocity ω is large, it indicates that the carrier's rotational motion is intense, and the error in the accelerometer's attitude measurement is large. In this case, it is desirable to perform the next measurement update after a longer period of time. Therefore, the filter period for calculating the measurement update is set to adapt to different motion states of the carrier.

[0095] After each measurement update is completed, the filtering period is calculated to determine the next measurement update. Steps two through five are executed within the current filtering period.

[0096] The method of this invention can effectively utilize the differences in attitude calculation by gyroscope and acceleration when the strapdown inertial attitude is significantly accumulated due to strong vibration of the carrier. Through strapdown attitude error judgment, adaptive Kalman filtering and other operations, it can improve the calculation accuracy of the carrier's pitch and roll angles under vibration environment, and also improve the calculation accuracy of the heading angle.

[0097] In general, the various exemplary embodiments of this disclosure can be implemented in hardware or dedicated circuitry, software, firmware, logic, or any combination thereof. Some aspects can be implemented in hardware, while others can be implemented in firmware or software executed by a controller, microprocessor, or other computing device. When aspects of embodiments of this disclosure are illustrated or described as block diagrams, flowcharts, or using some other graphical representation, it will be understood that the blocks, apparatuses, systems, techniques, or methods described herein can be implemented as non-limiting examples in hardware, software, firmware, dedicated circuitry or logic, general-purpose hardware or controllers or other computing devices, or some combination thereof.

[0098] Except for the technical features described in the specification, all other technologies are known to those skilled in the art. Descriptions of well-known components and technologies are omitted in this invention to avoid redundancy and unnecessary limitation. The embodiments described above do not represent all embodiments consistent with this application. Various modifications or variations that can be made by those skilled in the art without creative effort based on the technical solutions of this invention are still within the protection scope of this invention.

Claims

1. A vibration-resistant attitude filtering method based on inertial measurement, wherein an inertial sensor is installed inside a carrier and is fixedly connected to the carrier, and the three sensitive axes of the inertial sensor are aligned with the three axes of the carrier's coordinate system; characterized in that, The method includes: Step 1: Set the system state variable X = [X1X2X3] T , X1=[θφψ];X2=[b gx b gy b gz ];X3=[S x S y S z ]; Where θ, φ, and ψ represent the vehicle's pitch angle, roll angle, and yaw angle, respectively; b gx b gy b gz These represent the three-axis components of the gyroscope's zero bias; S x S y S z These represent the three-axis scale factors of the gyroscope; [·] T Indicates matrix transpose; Step 2: Construct the system state equations as follows: Among them, matrix X1(1) and X1(2) represent the first and second elements of vector X1, respectively; The values ​​are the three-axis measurements of the gyroscope; w = [n gx n gy n gz ] T This represents the noise level in the three-axis measurement of the gyroscope. Obtain the current three-axis measurement value of the gyroscope, perform time update steps according to the system state equation, predict the current system state X based on the system state at the previous moment, and obtain the predicted carrier attitude X1 at the current moment. Step 3: Calculate the carrier attitude based on the accelerometer measurement data; assume the three-axis accelerometer measurement values ​​are... The pitch angle θ was calculated. acc and roll angle φ acc as follows: Where g is the acceleration due to gravity; Step 4: Calculate the upper bound of Euler angle fluctuation based on the error Δa, including: calculating the current error. Next, calculate the pitch and roll angles of the fluctuation when Δa is distributed to different axes of the accelerometer, and determine the upper bound R of the pitch angle fluctuation. θ The upper bound R of the roll angle fluctuation φ ; Step 5: Determine whether the predicted carrier attitude at the current moment meets the following conditions: If the conditions are met, it indicates that the current predicted carrier attitude is accurate, and the error requirement assessment continues; if the conditions are not met, it indicates that the current predicted carrier attitude is inaccurate, and the assessment continues to determine whether the current filtering period T has been reached. S If the destination is not reached, proceed to step two; if the destination is reached, perform an error requirement assessment. The error requirement judgment refers to determining whether the current error Δa is less than the set threshold. If yes, continue to step six; otherwise, continue to step two. Step 6: Calculate the pitch angle θ from the accelerometer measurements. acc and roll angle φ acc As the observation vector, establish the system observation equation; Step 7: Calculate the observation noise covariance matrix R based on the average angular velocity ω; First, calculate the average angular velocity. Next, calculate the weights. And 0.1 < k ω <10; then calculate the matrix. Where k0 and k1 are both set coefficients; The measurement update step of Kalman filtering is performed, the Kalman gain matrix is ​​calculated based on the observation noise covariance matrix R calculated at the current time, and then the predicted system attitude X at the current time is corrected. Step 8: Set the current filter period T based on the average angular velocity ω. S as follows: Where k2 is the set coefficient; Perform steps two through five within the current filtering cycle.

2. The method according to claim 1, characterized in that, In step four, calculating the upper bound of the Euler angle fluctuation includes: distributing the current error Δa to different axes of the accelerometer, and calculating the pitch and roll angles of the fluctuation as follows: ① Assigning Δa to the x-axis, we obtain the pitch and roll angles of the wave as follows: ② Assigning Δa to the z-axis, the pitch and roll angles of the wave are obtained as follows: ③ Assigning Δa to the y-axis, we obtain the pitch and roll angles of the wave as follows: Then calculate the upper bound R of the pitch angle fluctuation. θ The upper bound R of the roll angle fluctuation φ as follows:

3. The method according to claim 1, characterized in that, In step five, the threshold for error Δa is set to 0.1 m / s. 2 .

Images