Anti-vibration attitude and heading filtering method based on inertial measurement

Abstract

The invention provides an anti-vibration attitude and heading filtering method based on inertial measurement, and belongs to the technical field of inertial navigation. The method comprises the following steps: fixedly connecting an inertial sensor in a carrier, constructing a system state variable based on a carrier attitude, and predicting a carrier state of a current time step based on a current gyroscope measurement value and a state equation; calculating carrier attitude according to the measured value of the accelerometer and taking the carrier attitude as an observation vector; calculating the fluctuation upper bound of the attitude angle according to the acceleration error, judging whether the current predicted carrier state is accurate, whether the current filter period is reached and whether the current acceleration error meets the requirement, and if the current predicted carrier state is accurate and the error meets the requirement, weighting and calculating an observation noise covariance matrix according to the current average angular velocity of the gyroscope; and carrying out measurement updating and filtering period updating. The method can be suitable for the situation that a carrier has strong vibration, the attitude and heading measurement accuracy is improved, and error parameters of the gyroscope and the acceleration do not need to be calibrated.

Description

Technical Field

[0001] The present invention relates to the technical field of inertial navigation, and particularly to an attitude filtering method based only on inertial measurement in a vibration environment. Background Art

[0002] The basic principle of inertial navigation technology is to measure acceleration and angular velocity, combine with the known initial state, and use a mathematical model for integral calculation to estimate the position, velocity and attitude of the target in real time. The inertial navigation system collects inertial data through its own sensitive devices, does not require external data participation, and does not radiate energy to the outside, having the advantages of high concealment, strong anti-interference ability, and complete autonomy.

[0003] However, since the positioning solution of the inertial navigation system is obtained by integrating acceleration and angular velocity, its error will accumulate and diverge over time and cannot be eliminated. Especially for low-cost micro-electro-mechanical system (MEMS) inertial navigation systems, due to their low measurement accuracy, errors are more likely to accumulate, affecting the long-term stability and accuracy of the system.

[0004] When the carrier is stationary or moving in a uniform straight line relative to the inertial reference system, the measurement data of the accelerometer can be used to calculate the attitude of the carrier. This attitude calculation method is instantaneous, and its error will not accumulate over time, thus effectively solving the problem of error accumulation generated when calculating the attitude by integrating angular velocity in the inertial navigation system.

[0005] However, in practical applications, the errors of the carrier attitude angle calculated by the above method mainly come from two aspects: one is the error in the measurement of the accelerometer, and the other is that in actual situations, the carrier usually does not satisfy the condition of being stationary or moving in a uniform straight line relative to the inertial system, so the conditions for the above formula to hold are not strictly satisfied. Therefore, the technology for obtaining the carrier attitude only relying on inertial measurement needs to be improved to ensure the accuracy of carrier attitude measurement. Summary of the Invention

[0006] In order to solve the problem of inaccurate measurement of the carrier attitude angle in the above practical applications, the present invention provides an anti-vibration attitude filtering method based on inertial measurement, aiming to ensure the attitude accuracy of a low-cost inertial navigation system during long-term independent application and when the carrier has strong vibrations only relying on inertial measurement.

[0007] An anti-vibration attitude filtering method based on inertial measurement provided by the present invention, the inertial sensor applied is installed in the carrier, the inertial sensor is fixedly connected to the carrier, and the three sensitive axes of the inertial sensor point in the same direction as the three axes of the carrier coordinate system. The method of the present invention uses the Kalman filtering method to construct an attitude filtering model, including 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 ψ respectively represent the pitch angle, roll angle, and heading angle of the carrier; b gx , b gy , b gz respectively represent the three-axis components of the gyroscope zero bias; S x , S y , S z respectively represent the three-axis scale factors of the gyroscope.

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

[0015]

[0016] Where, are the three-axis measurement values of the gyroscope; represents the gyroscope measurement noise; X1(1) and X1(2) respectively represent the first element and the second element of the vector X1.

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

[0018] Step 3: Calculate the carrier attitude based on the accelerometer measurement data as follows:

[0019]

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

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

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

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

[0024]

[0025] If it is satisfied, it means that the currently predicted carrier attitude is accurate, and continue to judge the error requirement; if not, it means that the currently predicted carrier attitude is inaccurate, and continue to judge whether the current moment has reached the current filtering period T S . If it has not reached, continue to execute Step Two. If it has reached, judge the error requirement. The so-called judging the error requirement means judging whether the current error Δa is less than the set threshold. If so, continue to execute Step Six. If not, continue to execute Step Two.

[0026] Step Six: Take the pitch angle θ acc and roll angle φ acc calculated by the accelerometer measurement as the observation vector, and construct the system observation equation 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 Seven: Calculate the observation noise covariance matrix R based on the weighted average angular velocity ω, including:

[0030] First, calculate the average angular velocity Secondly, calculate the weight and 0.1 < k ω < 10; then calculate the matrix where k0 and k1 are both set coefficients.

[0031] Execute the measurement update step of the Kalman filter, calculate the Kalman gain matrix based on the observation noise covariance matrix R calculated at the current moment, and then correct the predicted system state X at the current moment.

[0032] Step Eight: Set the current filtering period T according to the average angular velocity ω S as follows:

[0033] where k2 is a coefficient, and the unit of T S is seconds. ​

[0034] Execute steps two to five within the current filtering period.

[0035] Compared with the prior art, the advantages and positive effects of the present invention are as follows: The method of the present invention uses Kalman filtering to calculate the pitch angle and roll angle from the measurement data of the accelerometer, takes them as system observation quantities, and corrects the attitude by reasonably setting the observation noise covariance matrix, thereby improving the attitude estimation accuracy of the system. The method of the present invention can be applied under the condition of strong vibration of the airframe, without calibrating the error parameters of the gyroscope and accelerometer, and only relying on inertial measurement ensures the attitude accuracy of the low-cost inertial navigation system during long-term independent application and strong vibration of the airframe. The method of the present invention is simple and practical, and has a significant effect on improving the accuracy of attitude measurement. Brief Description of the Drawings

[0036] Figure 1 It is a schematic flow chart of the attitude and heading filtering method based on inertial measurement of the present invention. Detailed Embodiments

[0037] The following further describes the present invention in conjunction with the drawings and specific embodiments, so that those skilled in the art can better understand the present invention and be able to implement it, but the embodiments given are not intended to limit the present invention.

[0038] In the application scenario of the embodiment of the present invention, the inertial navigation system is installed on an unmanned aerial vehicle, and the installation point is located at the center of mass of the unmanned aerial vehicle carrier. The sensitive directions of the inertial sensors are the same as the three-axis directions of the carrier coordinate system. The unmanned aerial vehicle normally flies to collect data, and the method of the present invention is used to solve the attitude of the unmanned aerial vehicle. As Figure 1 shown, a method for attitude and heading filtering based on inertial measurement disclosed by the present invention designs an attitude and heading filtering model based on Euler angles, calculates the pitch angle and roll angle from the measurement data of the accelerometer, takes them as the observation quantities of the system, and realizes the filtering and correction of the attitude by reasonably setting the observation noise covariance matrix. The method of the present invention can be used under the condition of strong vibration of the airframe, and there is no need to calibrate the error parameters of the gyroscope and accelerometer. It is simple and practical and has a significant effect. The method of the present invention includes the following nine steps.

[0039] Step 1: 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 pitch angle, roll angle, and heading angle of the carrier respectively; b gx , b gy , b gz represent the three-axis components of the gyroscope zero bias respectively; S x , S y , S z represent the three-axis scale factors of the gyroscope respectively; [·] T represents matrix transpose.

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

[0046]

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

[0048] According to the relationship between the Euler angle change rate and the gyroscope measurement value in the following formula, the embodiment of the present invention determines the system state equation as follows:

[0049]

[0050] Where the gyroscope measurement value represent the three-axis measurement values of the gyroscope respectively.

[0051] In this step, the state at the current moment is predicted based on the state at the previous moment and the current gyroscope measurement value, that is, the time update of the Kalman filter is performed to obtain the predicted pitch angle, roll angle, and heading angle of the carrier. At the initial moment, the first observation value can be directly used as the initial state, or it can also be set through prior measurement values.

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

[0053]

[0054] Where k is the filtering time; is based on the state at time k Predict the state at time k+1; Φ(k+1,k) is the state transfer 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 by the state equation; Q k is the process noise covariance matrix at time k.

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

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

[0057]

[0058] Among them, a x ,a y ,a z are the ideal measurement values ​​of the three axes of the accelerometer; g is the acceleration due to gravity.

[0059] According to the output of the accelerometer, the pitch angle θ can be calculated back acc and roll angle φ acc , as follows:

[0060]

[0061] in Respectively represent the three-axis measurement values ​​of the accelerometer.

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

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

[0064] (2) Calculate the fluctuation of the Euler angle when Δa is distributed to different axes, as follows:

[0065] ① Distribute Δa to the x-axis, and obtain the pitch and roll angles of the fluctuation respectively:

[0066]

[0067] ② Distribute Δa to the z-axis, and obtain the pitch and roll angles of the fluctuation respectively:

[0068]

[0069] ③Distribute Δa to the y-axis, and the resulting fluctuating pitch angle and roll angle are respectively:

[0070]

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

[0072]

[0073] Step Five: 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, it is considered that the accuracy of the attitude predicted in the current Step Two is within an acceptable range.

[0075]

[0076] According to the above conditions, it can be judged whether the attitude angle predicted at the current time step is accurate.

[0077] If the attitude angle satisfies the above conditions at this time, continue to judge whether the error Δa meets the requirements. If Δa < 0.1 m / s 2 , it means that the requirements are met, and continue to execute Step Six for measurement update and filter period update; if Δa ≥ 0.1 m / s 2 , it means that the requirements are not met, and continue to execute Step Two, waiting for IMU input and executing the time update step.

[0078] If the attitude angle does not satisfy the above conditions at this time, first judge whether the filter period T of the last measurement update has been reached S . If not, continue to execute Step Two, waiting for IMU input and executing the time update step. If T S has been reached, then judge whether Δa meets the requirements. If Δa < 0.1 m / s 2 , enter Step Six for execution, for measurement update and filter period update. If Δa ≥ 0.1 m / s 2 , then continue to execute Step Two, waiting for IMU input and executing the time update step.

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

[0080]

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

[0082] Step Seven: Calculate the observation noise covariance matrix R by weighting 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 0.1 < k ω < 10, where k0 and k1 are pre-set coefficients.

[0085]

[0086] (3) Calculate the observation noise covariance matrix R as:

[0087]

[0088] Execute the measurement update step of the Kalman filter, correct the prediction result by combining the observation value, and the update process is as follows:

[0089]

[0090] Among them, 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 is the observation noise covariance matrix at the currently updated time k + 1; [·] -1 represents the inverse matrix; 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 Eight: Set the filtering period T according to the average angular velocity ω S , and set the upper and lower bounds 0.5 < T S ≤ 2, where k2 is a coefficient.

[0092]

[0093] The calculated T S is in seconds, and set the maximum filtering period to 2 seconds.

[0094] If the average angular velocity ω is small, it means that the rotational motion of the carrier is not intense, and the accuracy of the accelerometer for attitude measurement is relatively high, then the next measurement update is performed after a short time. If the average angular velocity ω is large, it means that the rotational motion of the carrier is intense, and the error of the accelerometer for attitude measurement is large, then it is desired to perform the next measurement update after a long time. Therefore, set the filtering period for calculating the measurement update to adapt to different motion states of the carrier.

[0095] After each measurement update is performed, calculate the filtering period for determining the execution of the next measurement update. Steps two to five are executed within the current filtering period.

[0096] The method of the present invention can effectively utilize the difference in attitude calculation between the gyroscope and the accelerometer in the case where the strapdown inertial attitude cumulative error is significant due to strong vibration of the carrier. By means of strapdown attitude error determination, adaptive Kalman filtering and other operations, the calculation accuracy of the pitch and roll angles of the carrier in a vibration environment is improved, and at the same time, the purpose of improving the calculation accuracy of the heading angle is also achieved.

[0097] In general, the various example embodiments of the present disclosure may be implemented in hardware or special-purpose circuits, software, firmware, logic, or any combination thereof. Some aspects may be implemented in hardware, while other aspects may be implemented in firmware or software executed by a controller, a microprocessor, or other computing device. When aspects of the embodiments of the present disclosure are illustrated or described as block diagrams, flowcharts, or using some other graphical representation, it will be understood that the blocks, devices, systems, techniques, or methods described herein may be implemented as non-limiting examples in hardware, software, firmware, special-purpose circuits 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, they are all well-known technologies to those skilled in the art. The present invention omits the description of well-known components and well-known technologies to avoid redundancy and unnecessary limitation of the present invention. The embodiments described in the above embodiments do not represent all embodiments consistent with the present application. On the basis of the technical solution of the present invention, various modifications or deformations that can be made by those skilled in the art without creative labor are still within the protection scope of the present invention.

Claims

1. An anti-vibration attitude filtering method based on inertial measurement, where the inertial sensor is installed inside the carrier, the inertial sensor is fixedly connected to the carrier, and the three sensitive axes of the inertial sensor point in the same directions as the three axes of the carrier coordinate system; characterized in that, The method includes: Step 1: Set the system state variable X = [X1 X2 X3] T , X1 = [θ φ ψ]; X2 = [b gx b gy b gz ; X3 = [S x S y S z ; where θ, φ, and ψ represent the pitch angle, roll angle, and heading angle of the vehicle, respectively; b gx , b gy , b gz represent the three-axis components of the gyroscope zero bias, respectively; S x , S y , S z represent the three-axis scale factors of the gyroscope, respectively; [·] T represents matrix transpose; Step 2: Construct the system state equation as follows: Among them, the matrix X1(1) and X1(2) represent the first element and the second element of vector X1 respectively; is the triaxial measurement value of the gyroscope; w = [n gx n gy n gz T represents the triaxial measurement noise of the gyroscope;​ Obtain the current three-axis measurement values of the gyroscope, perform a time update step according to the system state equation, predict the current system state X based on the previous system state, and obtain the predicted vehicle attitude X1 at the current moment; Step 3: Calculate the attitude of the carrier based on the accelerometer measurement data. Let the three-axis measurement values of the accelerometer be The pitch angle θ is calculated acc and the 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 Then calculate the pitch angle and roll angle of the fluctuation when Δa is distributed to different axes of the accelerometer, and determine the upper bound R of the pitch angle fluctuation θ and the upper bound R of the roll angle fluctuation φ ; Step Five: Determine whether the predicted vehicle attitude at the current moment meets the following conditions: If it is satisfied, it means that the currently predicted carrier attitude is accurate, and continue to judge the error requirement; if it is not satisfied, it means that the currently predicted carrier attitude is inaccurate, and continue to judge whether the current time has reached the current filtering period T S , if it has not reached, continue to execute step 2, if it has reached, judge the error requirement; The determination of the error requirement refers to determining whether the current error Δa is less than the set threshold. If so, continue to execute Step Six; if not, continue to execute Step Two; Step 6: Take the pitch angle θ acc and roll angle φ acc calculated from the accelerometer measurements as the observation vector and establish the system observation equation; Step Seven: Calculate the observation noise covariance matrix R based on the weighted average angular velocity ω; First, calculate the average angular velocity Second, calculate the weight And 0.1 < k ω < 10; then calculate the matrix where both k0 and k1 are set coefficients; Execute the measurement update step of the Kalman filter, calculate the Kalman gain matrix based on the observation noise covariance matrix R calculated at the current moment, and then correct the predicted system attitude X at the current moment; Step Eight: Set the current filtering period T according to the average angular velocity ω S As follows: where k2 is a set coefficient; Execute Steps Two to Five within the current filtering period.

2. The method according to claim 1, wherein In Step Four described above, calculating the upper bound of the Euler angle fluctuation includes: distributing the current error Δa to different axes of the accelerometer, and calculating the fluctuating pitch angle and roll angle as follows: ① Distribute Δa to the x-axis, and the resulting fluctuating pitch angle and roll angle are: ② Distribute Δa to the z-axis, and the resulting fluctuating pitch angle and roll angle are: ③ Distribute Δa to the y-axis, and the resulting fluctuating pitch angle and roll angle are: Then calculate the upper bound of the pitch angle fluctuation, R θ and the upper bound of the roll angle fluctuation, R φ as follows:

3. The method according to claim 1, wherein In step 5 described above, set the threshold of the error Δa to 0.1 m / s 2 .

Images