CN102692225A - Attitude heading reference system for low-cost small unmanned aerial vehicle - Google Patents

Abstract

The invention relates to an attitude and heading reference system for low-cost small unmanned aerial vehicles, including an angular rate gyro, an accelerometer, GPS, an angular rate gyro computing module, a correction module and a Kalman filter, wherein the angular rate gyro measures the aircraft's Roll angular rate, pitch angular rate and yaw angular rate; the accelerometer measures the component of gravity under the coordinate axis of the body; the GPS measures the track azimuth of the aircraft; the angular rate gyro calculation module obtains the roll angle f ₁ with offset, Pitch angle q ₁ and yaw angle y ₁ ; the correction module obtains the estimated roll angle f ₂ , pitch angle q ₂ , and takes the track azimuth angle obtained from GPS measurement as the estimated yaw angle y ₂ ; the Kalman filter will The data generated by the angular rate gyro operation module and the correction module are fused to obtain the final attitude angle [fqy] ^T . The invention reduces the cost of the small unmanned aerial vehicle system; reduces the complexity of the heading reference system; and improves the accuracy of heading attitude estimation.

Description

An Attitude and Heading Reference System for Low-Cost Small Unmanned Aerial Vehicles

technical field

The invention relates to an attitude and heading reference system, in particular to an attitude and heading reference system for low-cost small UAVs based on sensor fusion technology.

Background technique

Attitude Heading Reference System (AHRS), referred to as the heading system, is used to determine the orientation of the moving carrier in space. The attitude information provided by the attitude heading reference system can be widely used in navigation, guidance and control. For autonomous flying UAVs, the heading and attitude system is particularly important, which provides the necessary attitude feedback for the inner loop of the control system. Usually, the determination of attitude information relies on high-precision angular rate gyroscopes. However, high-precision gyroscopes have defects such as high cost, heavy weight, and complex structure, which obviously cannot be applied to low-cost small UAV systems. Therefore, under the constraints of low-cost sensors, providing an attitude and heading reference system with a small amount of calculation and high reliability for small UAV systems has always been an issue of extensive research in related fields at home and abroad.

Usually, the pose of a moving vehicle is described by Euler angles (f, q, y). According to the relationship between the angular rate of the carrier motion and the rate of change of the Euler angle, the measured value of the angular rate gyro can be integrated to obtain the attitude information of the carrier, but this method has higher requirements for the accuracy of the gyroscope (the output error is less than 0.1°/h). Low-cost small drones usually use MEMS (Micro Electrical Mechanical System) sensors. Although they have the characteristics of low cost and small size, the accuracy of this type of sensor is poor, and the output error is between 10° and 100°/h. The integral gyro output When the value is large, there will be a large drift, so it cannot be directly used for attitude estimation.

The attitude information can also be determined by using the observation vector instead of depending on the angular rate gyroscope. The basic principle of using the observation vector to determine the attitude is: use two non-collinear observation vectors in the two coordinate systems, make the two coordinate systems overlap through three rotations, and then calculate the Euler angle. Some literatures have conducted research on such Gyro-Free Systems. For example, using accelerometers, magnetometers, and GPS (Global Positioning System) as sensors, attitude information can be obtained by solving the Wahba problem; attitude estimation can also be performed using GPS carrier phase observations through multi-antenna GPS. However, in many practical situations, this gyro-free attitude system cannot be used alone. First of all, the gyro-free attitude system cannot provide high-bandwidth attitude information; secondly, when the carrier performs a specific maneuver, the attitude information estimated by this method will have a large error. For example, when the UAV tilts and turns, the accelerometer will be sensitive to the centripetal acceleration other than the acceleration of gravity, resulting in inaccurate calculation of attitude; finally, the attitude information given by the gyro-free attitude system contains large noise.

Based on the above reasons, the sensor fusion (Sensor Fusion) technology can be used to integrate the angular rate gyro with the gyro-free system composed of accelerometer, magnetometer and GPS, so that they complement each other and learn from each other. Because the angular rate gyro can provide high-bandwidth biased attitude information, it can be used to "smooth" the gyro-free system; and the gyro-free system is used to correct the drift generated by the angular rate gyro, so it is also called an aiding system. Thus, combining the two with appropriate filters yields a high-bandwidth and unbiased solution to pose information. The core of sensor fusion technology is the filter used to fuse different sensors. The method of using filter fusion angular rate gyroscope and gyroscope system for attitude measurement has been widely used in satellite attitude determination for decades.

For low-cost small UAVs, although sensor fusion technology has been widely used in attitude determination, there are still some problems: First, the sensors used for attitude estimation in small UAV systems are generally angular rate gyroscopes, accelerometers, magnetic Heading (used to measure the heading angle), while the magnetic heading is expensive and not suitable for low-cost UAV systems; secondly, although some small UAV attitude estimation systems use GPS instead of magnetic heading, which reduces the cost , but since the GPS output is an angle, the attitude estimation algorithm at this time is based on the Euler angle method. This method needs to solve the trigonometric function in real time during the calculation, and sometimes there will be a singularity, which makes the problem There is no solution, so it is not suitable for low-cost flight control computers with poor computing power. At present, there is no solution that can better solve the attitude estimation when the UAV appears to be maneuvering and overloaded.

Contents of the invention

The purpose of the present invention is to address the shortcomings of the prior art and provide a set of attitude and heading reference system solutions, which can realize the attitude information estimation of small UAVs only by low-cost sensors including angular rate gyroscopes, GPS, and accelerometers.

The present invention provides an attitude and heading reference system for low-cost small unmanned aerial vehicles, including an angular rate gyro, an accelerometer, GPS, an angular rate gyro computing module, a correction module and a Kalman filter, wherein:

The angular rate gyro measures the roll rate, pitch rate and yaw rate of the aircraft;

The accelerometer measures the component of gravity under the coordinate axis of the body;

GPS measures the track azimuth of the aircraft;

The angular rate gyro calculation module obtains the roll angle f ₁ , the pitch angle q ₁ and the yaw angle y ₁ with offset through the roll angular rate, pitch angular rate and yaw angular rate measured by the angular rate gyro;

The correction module obtains the roll angle f ₂ and the pitch angle q ₂ estimated by the correction module through the body axis component of the gravitational acceleration measured by the accelerometer, and uses the track azimuth obtained by the GPS measurement as the estimated yaw angle y ₂ ;

The Kalman filter fuses the data generated by the angular rate gyro operation module and the correction module to obtain the final attitude angle [f q y] ^T .

Beneficial effect

The invention has the advantages of: reducing the cost of the small unmanned aerial vehicle system by using low-cost sensors; reducing the complexity of the traditional attitude and heading reference system; relying on sensor fusion technology to improve the accuracy and reliability of the system's attitude estimation ; Solve the problem of inaccurate attitude estimation when the UAV is overloaded.

Description of drawings

Fig. 1 is a structural diagram of the attitude heading reference system;

Figure 2 is the roll angle solution under static conditions;

Figure 3 shows the pitch angle solution under static conditions;

Fig. 4 is angular rate gyro drift estimation;

Fig. 5 is roll angle error and its standard deviation;

Fig. 6 is the roll angular rate gyro drift error and its standard deviation;

同真实滚转角f之间的误差对比；Figure 7 shows the estimated roll angle

Compared with the error between the real roll angle f;

Figure 8 is the roll angle estimation based on the flight test, where (a) is the roll angle estimated by the time window method, and (b) is the roll angle estimated by conventional filtering and gyro integration;

Fig. 9 is the pitch angle estimation based on the flight test, wherein (a) is the pitch angle estimated by the time window method, and (b) is the pitch angle estimated by conventional filtering and gyro integration;

Fig. 10 is the filter switch sign quantity;

Figure 11 shows the trigger value and threshold value of the filter switch.

Detailed ways

The preferred embodiments of the present invention will be specifically described below in conjunction with the accompanying drawings.

This embodiment implements an attitude and heading reference system for a low-cost small unmanned aerial vehicle, which consists of three parts: an angular rate gyro subsystem, a correction subsystem, and a Kalman filter. Figure 1 shows the structure diagram of the attitude heading reference system. In this embodiment, the error quaternion method is used to establish the model, which relieves the calculation load of the onboard processor.

1) Angular rate gyro operation module

The attitude of a rigid body in three-dimensional space is usually described by three Euler angles, which are roll angle f, pitch angle q, and yaw angle y. Attitude angles describe the relative relationship between two different coordinate systems. In navigation, guidance and control, for an aircraft moving near the ground, the body coordinate axis S _b -Oxyz system is usually selected as the dynamic coordinate system, and the navigation coordinate axis S _n -Ox _n y _n z _n is selected as the reference coordinate system, Defined as the North-East-Down (NED) reference system, x _n is north, y _n is east, and z _n points to the center of the earth.

Define an arbitrary vector u, which is denoted as u ⁿ and u ^b in the navigation coordinate system and the body coordinate system respectively. The projection transformation of the vector u in the above two different coordinate axes is realized by the direction cosine matrix:

${\mathrm{<span\; class="notranslate">\; u</span>}}^{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}^{\mathrm{<span\; class="notranslate">\; b</span>}}{\mathrm{<span\; class="notranslate">\; u</span>}}^{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; )</span>$

为方向余弦矩阵(Direction Cosine Matrix，DCM)，也称为姿态矩阵，通常以欧拉角的形式定义。In the formula

is the direction cosine matrix (Direction Cosine Matrix, DCM), also known as the attitude matrix, usually defined in the form of Euler angles.

In practical applications, there are several problems in using Euler angles to represent poses. First, in some special cases, the individual attitude angles become uncertain, and the kinematic equations appear singular. For example, when the pitch angle q=±90°, the yaw angle y is uncertain, and the equation dy/dt has singularity sex. Secondly, in the actual solution, the Euler angle method needs to solve a large number of trigonometric functions, which will inevitably bring a computational burden to the processor. If the attitude quaternion method is used, on the one hand, the singularity of the motion equation can be avoided, and on the other hand, there is no need to calculate trigonometric functions, which saves processor resources and improves calculation efficiency. In order to overcome the above disadvantages, this embodiment uses the quaternion method for modeling.

Define the pose quaternion q:

$\mathrm{<span\; class="notranslate">\; q</span>}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}\\ \stackrel{\mathrm{<span\; class="notranslate">\; r</span>}}{\mathrm{<span\; class="notranslate">\; q</span>}}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; )</span>$

称为四元数q的矢量部分。用四元数表示的方向余弦阵定义如下：where q ₀ is called the scalar part of the quaternion q,

The vector part called the quaternion q. The direction cosine array represented by a quaternion is defined as follows:

${\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}^{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; q</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="2"\; label="q"\; filenames="HDA0000051922760000012.png,HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="3"\; label="q"\; filenames="HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span>& \mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="2"\; label="q"\; filenames="HDA0000051922760000012.png,HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; )</span>& \mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; )</span>\\ \mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; )</span>& \mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="1"\; label="q"\; filenames="HDA0000051922760000012.png,HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="3"\; label="q"\; filenames="HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span>& \mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="3"\; label="q"\; filenames="HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span>\\ \mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="3"\; label="q"\; filenames="HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; )</span>& \mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; )</span>& \mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="1"\; label="q"\; filenames="HDA0000051922760000012.png,HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="2"\; label="q"\; filenames="HDA0000051922760000012.png,HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span>\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; )</span>$

The quaternion differential equation is determined by:

Among them, p is the roll rate, q is the pitch rate, and r is the yaw rate.

From the quaternion differential equation (4), it can be seen that the quaternion can be updated in real time through the measured value of the angular rate gyroscope, and then the attitude angle of the carrier can be obtained according to the direction cosine matrix (3) represented by the attitude quaternion

$\text{<span class="notranslate"> f=arctan</span>}\frac{\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="3"\; label="q"\; filenames="HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span>}{\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="1"\; label="q"\; filenames="HDA0000051922760000012.png,HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="2"\; label="q"\; filenames="HDA0000051922760000012.png,HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span>}$

q＝arcsin 2(-q ₁ q ₃ +q ₂ q ₀ ) (5)

$\text{<span class="notranslate"> y=arctan</span>}\frac{\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="2"\; label="q"\; filenames="HDA0000051922760000012.png,HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; )</span>}{\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="2"\; label="q"\; filenames="HDA0000051922760000012.png,HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; <figure-callout\; id="3"\; label="q"\; filenames="HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout></span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span>}$

2) Calibration module

Although there is an unbounded error in the attitude angle obtained by integrating the angular rate gyro measurements, the angular rate gyro is indispensable for most AHRS systems because it can provide a high-bandwidth output. The drift caused by gyro integration can be suppressed by the correction system, because the sensors that make up the correction system have the characteristic of bounded output error. The correction system can periodically "reset" the attitude information provided by the angular rate gyro to achieve the purpose of correction. In this embodiment, the accelerometer and the GPS are selected to form a correction system together.

The attitude angle can be obtained by observing the projection of the gravity vector on the body coordinate axis system by the accelerometer. According to the principle of the accelerometer, assuming that the output vector of the three axes is f ^b , it can be expressed

${\mathrm{<span\; class="notranslate">\; f</span>}}^{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; a</span>}}_{\mathrm{<span\; class="notranslate">\; ib</span>}}^{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; g</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 6</span>}<span\; class="notranslate">\; )</span>$

为载体相对于惯性系的运动加速度在机体坐标轴系下的分量，g_b为重力加速度矢量在机体坐标轴系下的分量in,

is the component of the motion acceleration of the carrier relative to the inertial system under the body coordinate axis system, g _b is the component of the gravity acceleration vector under the body coordinate axis system

${\mathrm{<span\; class="notranslate">\; g</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; C</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}^{\mathrm{<span\; class="notranslate">\; b</span>}}{\mathrm{<span\; class="notranslate">\; g</span>}}_{\mathrm{<span\; class="notranslate">\; no</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; g</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; q</span>}\\ \mathrm{<span\; class="notranslate">\; g</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; f</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; q</span>}\\ \mathrm{<span\; class="notranslate">\; g</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; f</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; q</span>}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 7</span>}<span\; class="notranslate">\; )</span>$

Wherein, is the projection of the gravitational acceleration vector under the navigation coordinate axis system, which can be expressed as g _n =[0 0 g] ^T , and g is the gravitational acceleration constant.

时，即载体静止或做匀速直线运动时，f^b变为exist

When , that is, when the carrier is stationary or moving in a straight line at a uniform speed, f ^b becomes

${\mathrm{<span\; class="notranslate">\; f</span>}}^{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}\\ {\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}}\\ {\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; z</span>}}\end{array}\right]<span\; class="notranslate">\; =</span><span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; g</span>}}_{\mathrm{<span\; class="notranslate">\; b</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}\mathrm{<span\; class="notranslate">\; g</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; q</span>}\\ <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; g</span>}\mathrm{<span\; class="notranslate">\; sin</span>}\mathrm{<span\; class="notranslate">\; f</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; q</span>}\\ <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; g</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; f</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; q</span>}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 8</span>}<span\; class="notranslate">\; )</span>$

At this time, the pitch angle q and roll angle f can be determined by the above formula

$q=\arcsin \frac{f_x}{g}$ (9)

$\mathrm{<span\; class="notranslate">\; f</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; arcsin</span>}\frac{<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}}}{\mathrm{<span\; class="notranslate">\; g</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; q</span>}}$

Equation (8) does not include the yaw angle y, so only the pitch angle q and the roll angle f can be determined only by the accelerometer. In order to determine the yaw angle y, it is also necessary to introduce the heading signal provided by GPS. It should be pointed out that GPS can only provide track azimuth But it can be used instead of the yaw angle in the case of small crosswinds.

3) Kalman filter

In this embodiment, the Kalman filter is selected as the sensor fusion algorithm that combines the angular rate gyroscope and the correction system.

The working steps of the filter are: after the initial conditions are determined, the attitude estimation can be performed by using the measurement data integration of the angular rate gyroscope. This step is called "time update", which provides high-bandwidth attitude information. In the time update stage, due to the noise in the gyro data, the error of the estimated attitude accumulates with time, so the attitude cannot be determined simply by integrating the gyro data. In order to suppress the gyro error, it is necessary to introduce a correction system to periodically correct the gyro data. This step is called "measurement update". The covariance matrix of the state error and the gyro drift estimate are also corrected in this step. Next, a new round of time update is performed, which is repeated periodically.

和误差四元数q_e相乘的形式，即认为误差四元数q_e是估计四元数

到真实四元数q的旋转。由于角速率陀螺的误差，误差四元数是非零的小量。q_e，

和q的关系由下式确定：The true pose quaternion q can be expressed as the estimated quaternion

The form of multiplying the error quaternion q _e , that is, the error quaternion q _e is the estimated quaternion

Rotation to real quaternion q. Due to the error of the angular rate gyro, the error quaternion is a small non-zero quantity. q _e ,

The relationship with q is determined by the following formula:

$\mathrm{<span\; class="notranslate">\; q</span>}<span\; class="notranslate">\; =</span>\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}<span\; class="notranslate">\; \&CircleTimes;</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; e</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 10</span>}<span\; class="notranslate">\; )</span>$

The error quaternion can be approximated as:

${\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; e</span>}}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}\mathrm{<span\; class="notranslate">\; 1</span>}\\ {\stackrel{\mathrm{<span\; class="notranslate">\; r</span>}}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; e</span>}}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 11</span>}<span\; class="notranslate">\; )</span>$

Deriving the formula (11), and using the quaternion differential equation (4), after linearization, the quaternion error differential equation can be obtained

表示角速率陀螺测量值与真实值之差，

由下式确定in,

Indicates the difference between the measured value of the angular rate gyro and the true value,

determined by

$<span\; class="notranslate">\; [</span>\stackrel{\mathrm{<span\; class="notranslate">\; r</span>}}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}<span\; class="notranslate">\; \&times;</span><span\; class="notranslate">\; ]</span><span\; class="notranslate">\; =</span>\left[\begin{array}{ccc}\mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; r</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; q</span>}\\ <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; r</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}& \mathrm{<span\; class="notranslate">\; p</span>}\\ \mathrm{<span\; class="notranslate">\; q</span>}& <span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; p</span>}& \mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 13</span>}<span\; class="notranslate">\; )</span>$

The observation of the correction subsystem is taken as the error angle, which can be obtained by the following method: firstly, the attitude angle of the carrier is calculated according to formula (9) using the measurement information of the accelerometer and GPS, and then the attitude angle is calculated with the attitude angle obtained by formula (5). Subtract the angle estimates to get the angle error

$\left[\begin{array}{c}\mathrm{<span\; class="notranslate">\; \&delta;\; f</span>}\\ \mathrm{<span\; class="notranslate">\; \&delta;q</span>}\\ \mathrm{<span\; class="notranslate">\; \&delta;y</span>}\end{array}\right]<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; AS</span>}}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; AS</span>}}\\ {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; AS</span>}}\end{array}\right]<span\; class="notranslate">\; -</span>\left[\begin{array}{c}\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; f</span>}}\\ \stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}\\ \stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 14</span>}<span\; class="notranslate">\; )</span>$

为利用四元数法计算出的姿态角。In the formula, [δf δq δy] ^T is the error angle, [f _AS q _AS y _AS ] ^T is the attitude angle obtained by the correction system,

is the attitude angle calculated by the quaternion method.

The relationship between the angular error and the attitude quaternion is determined by the following formula

${\stackrel{\mathrm{<span\; class="notranslate">\; r</span>}}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; e</span>}}<span\; class="notranslate">\; =</span>{\left[\begin{array}{ccc}\frac{\mathrm{<span\; class="notranslate">\; \&delta;\; <figure-callout\; id="2"\; label="f"\; filenames="HDA0000051922760000012.png,HDA0000051922760000031.png"\; state="\{\{state\}\}">f</figure-callout></span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}& \frac{\mathrm{<span\; class="notranslate">\; \&delta;<figure-callout\; id="2"\; label="q"\; filenames="HDA0000051922760000012.png,HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout></span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}& \frac{\mathrm{<span\; class="notranslate">\; \&delta;y</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}\end{array}\right]}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 15</span>}<span\; class="notranslate">\; )</span>$

和三个角速率陀螺的漂移δb_p，δb_q和δb_r The state quantity of the correction subsystem is selected as the vector part of the error quaternion

and the drifts of the three angular rate gyros δb _p , δb _q and δb _r

$\mathrm{<span\; class="notranslate">\; x</span>}<span\; class="notranslate">\; =</span>{\left[\begin{array}{cccc}{\stackrel{\mathrm{<span\; class="notranslate">\; r</span>}}{\mathrm{<span\; class="notranslate">\; q</span>}}}_{\mathrm{<span\; class="notranslate">\; e</span>}}& \mathrm{<span\; class="notranslate">\; \&delta;</span>}{\mathrm{<span\; class="notranslate">\; b</span>}}_{\mathrm{<span\; class="notranslate">\; p</span>}}& \mathrm{<span\; class="notranslate">\; \&delta;</span>}{\mathrm{<span\; class="notranslate">\; b</span>}}_{\mathrm{<span\; class="notranslate">\; q</span>}}& \mathrm{<span\; class="notranslate">\; \&delta;</span>}{\mathrm{<span\; class="notranslate">\; b</span>}}_{\mathrm{<span\; class="notranslate">\; r</span>}}\end{array}\right]}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 16</span>}<span\; class="notranslate">\; )</span>$

Then the state equation of the system can be constructed

Among them, A(t) is the system matrix, defined as follows

$\mathrm{<span\; class="notranslate">\; A</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\left[\begin{array}{cc}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; [</span>\stackrel{\mathrm{<span\; class="notranslate">\; r</span>}}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}<span\; class="notranslate">\; \&times;</span><span\; class="notranslate">\; ]</span>& <span\; class="notranslate">\; -</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}{\mathrm{<span\; class="notranslate">\; I</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 3</span>}}\\ {\mathrm{<span\; class="notranslate">\; 0</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 3</span>}}& <span\; class="notranslate">\; -</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; \&tau;</span>}}{\mathrm{<span\; class="notranslate">\; I</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 3</span>}}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 18</span>}<span\; class="notranslate">\; )</span>$

The input matrix B(t) and the noise matrix W(t) are defined as follows

$\begin{array}{cc}\mathrm{<span\; class="notranslate">\; B</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; =</span>\left[\begin{array}{cc}<span\; class="notranslate">\; -</span>\frac{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; 2</span>}}{\mathrm{<span\; class="notranslate">\; I</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 3</span>}}& {\mathrm{<span\; class="notranslate">\; 0</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 3</span>}}\\ {\mathrm{<span\; class="notranslate">\; 0</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 3</span>}}& {\mathrm{<span\; class="notranslate">\; I</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}<span\; class="notranslate">\; \&times;</span>\mathrm{<span\; class="notranslate">\; 3</span>}}\end{array}\right]& \mathrm{<span\; class="notranslate">\; W</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; b</span>}}_{\mathrm{<span\; class="notranslate">\; w</span>}}\\ {\mathrm{<span\; class="notranslate">\; w</span>}}_{{\mathrm{<span\; class="notranslate">\; b</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}}\end{array}\right]\end{array}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 19</span>}<span\; class="notranslate">\; )</span>$

Equation (17) can be discretized into the following form

X _k+1 = F _k X _k + G _k W _k (20)

Among them, F _k is the state transition matrix, G _k is the system noise driving matrix

F _k =I+A(t)Δt (21)

G _k =B(t)Δt

Write the observation equation in the following discretized form

Z _k ＝H _k X _k +V _k (22)

Among them, Z=[δf δq δy] ^T is the observation quantity, V is the observation noise, H is the observation matrix, which can be determined by formula (15)

H＝[2I _3×3 0 _3×3 ](23)

In the measurement update stage of the Kalman filter, the attitude information provided by the angular rate gyro system is corrected by the accelerometer and GPS. The prerequisite for using the accelerometer to correct the gyro drift is that the carrier is at rest or in a state of uniform linear motion. Since there is no additional acceleration, the accelerometer is only sensitive to the component of the acceleration of gravity on the coordinate axis of the body. At this time, the formula (9) can be used Calculate the pitch angle q and roll angle f. However, when the vehicle is maneuvering, the attitude estimate is biased due to the additional acceleration sensed by the accelerometer. Therefore, a certain compensation method must be adopted to reduce the attitude angle error that occurs when the carrier maneuvers. Two solutions are proposed and analyzed below.

a) Overload compensation

When the aircraft makes a banked turn, due to the centripetal acceleration, there will be measurement errors in the accelerometer, making the attitude estimation inaccurate. For the convenience of analysis, the coordinated turning of the aircraft can be regarded as a circular motion.

The centripetal acceleration of a particle doing circular motion is determined by the following formula

a＝ω·V (24)

In the formula, ω represents the angular velocity of the particle around the center of the circle, and V represents the velocity of the particle. Obviously, the velocity vector, the angular velocity rotation axis and the acceleration vector are orthogonal. Outside the transition process, the velocity vector of the aircraft is considered to be along the x-axis direction of the airframe. In this way, since there is no velocity component in the y-axis and z-axis directions of the body, the accelerometer in the x-axis direction of the body will not be able to feel the centripetal acceleration. Both the angular rate gyro and the accelerometer are installed along the coordinate axis of the airframe, and the true airspeed V _TAS measured by the pitot tube is also along the x-axis direction of the airframe, so the measurement data of the angular rate gyro and the pitot tube can be directly used Calibrate the accelerometer. Since the accelerometer a _z along the direction of the z-axis of the body is not used in the Kalman filter algorithm, only the correction of the accelerometer a _y along the y-axis of the body is considered here.

Both the angular rates p and r in the x-axis and z-axis directions of the body will cause the acceleration q _y in the y-axis direction of the body. However, if the centripetal acceleration formula is written in the form of angular velocity around the center of the circle and the radius of the circle (as shown in the following formula), it can be seen that if the radius is small, the resulting acceleration will also be small.

a＝ω ² R (25)

The body roll caused by the deflection of the aileron is always along the direction of the x-axis of the body, and at the same time, the distance between the accelerometer a _y and the direction of the x-axis is relatively close. In this way, the acceleration in the y-axis direction caused by the angular velocity p around the x-axis can also be ignored, and the additional acceleration generated by turning is completely determined by the yaw angular velocity r around the z-axis direction and the true air speed V _TAS

a _y =r·V _TAS (26)

Therefore, the revised formula (9) can be written as follows

$q=\arcsin \frac{f_x}{g}$ (27)

$\mathrm{<span\; class="notranslate">\; f</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; arcsin</span>}\frac{<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; r</span>}{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; TAS</span>}}}{\mathrm{<span\; class="notranslate">\; g</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; q</span>}}$

A type of small unmanned aerial vehicle that performs detection tasks spends most of the time in a cruise state, and when it needs to change its course, it makes a banked turn. For this kind of unmanned aerial vehicle, the compensation scheme can achieve satisfactory results.

b) time window

When an aircraft is flying, it switches between cruising and maneuvering states. When cruising, it can be approximated that the aircraft is not overloaded, and the correction system at this time can be used to correct gyro drift. When the aircraft is manoeuvring, the correction system is not available due to interference from additional acceleration. Usually, the time for the aircraft to do continuous maneuvers is not very long, and the accumulation of gyro errors during this period is relatively small, so it can be considered to turn off the Kalman filter, and then turn it on when the maneuver is over.

作为触发卡尔曼滤波器的标志，时间窗口的开闭由下式确定When the aircraft turns, the angular rate will change significantly, so the norm of the angular rate is chosen

As a sign to trigger the Kalman filter, the opening and closing of the time window is determined by the following formula

$\left\{\begin{array}{c}\mathrm{<span\; class="notranslate">\; u</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&le;</span>\mathrm{<span\; class="notranslate">\; \&beta;</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; flag</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}\\ \mathrm{<span\; class="notranslate">\; u</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; ></span>\mathrm{<span\; class="notranslate">\; \&beta;</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; flag</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right.<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 28</span>}<span\; class="notranslate">\; )</span>$

Among them, β is the threshold value of the time window, and flag is the window flag.

When u(t)≤β, flag=1, the window is opened, and the Kalman filter is used for attitude determination; when u(t)>β, flag=0, the window is closed, and the filter is disconnected at this time, and the correction is stopped System fixes.

The specific implementation steps of attitude information estimation using the attitude heading reference system described in this embodiment are as follows:

Step 1: Calculate the attitude quaternion using the angular rate gyro measurement data

Step 2: Predict the state error covariance matrix according to F and G in formula (21)

${\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{<span\; class="notranslate">\; +</span>}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; G</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; Q</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; G</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 30</span>}<span\; class="notranslate">\; )</span>$

In the formula, Q _k-1 is the process noise covariance matrix.

Step 3: In the correction system, use (9) and the heading angle y _GPS measured by GPS to obtain the attitude angle information [f _AS q _AS y _AS ] ^T estimated by the correction system.

Step 4: Use the system attitude angle information estimated by the previous time step Make difference with the attitude angle information obtained by the correction system to obtain the observations of the Kalman filter

$\mathrm{<span\; class="notranslate">\; Z</span>}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}\mathrm{<span\; class="notranslate">\; \&delta;\; f</span>}\\ \mathrm{<span\; class="notranslate">\; \&delta;q</span>}\\ \mathrm{<span\; class="notranslate">\; \&delta;y</span>}\end{array}\right]<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; AS</span>}}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; AS</span>}}\\ {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; AS</span>}}\end{array}\right]<span\; class="notranslate">\; -</span>\left[\begin{array}{c}\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; f</span>}}\\ \stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}\\ \stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}\end{array}\right]<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 31</span>}<span\; class="notranslate">\; )</span>$

Step 5: Calculate the Kalman gain matrix

${\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}{\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; )</span>}^{<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 32</span>}<span\; class="notranslate">\; )</span>$

where R _k is the measurement noise covariance matrix.

Step 6: Update the state error covariance matrix

${\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; +</span>}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; I</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; )</span>{\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>}{<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; I</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}{\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 33</span>}<span\; class="notranslate">\; )</span>$

Step 7: Update the state vector

Step 8: Correct the attitude quaternion and angular rate gyro measurements from the time update step using the estimated quaternion and gyro drift

${\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; b</span>}}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; +</span>}<span\; class="notranslate">\; =</span>{\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; b</span>}}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&delta;</span>}{\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; b</span>}}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; +</span>}<span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; -</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; 35</span>}<span\; class="notranslate">\; )</span>$

${\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; +</span>}<span\; class="notranslate">\; =</span>{\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>}<span\; class="notranslate">\; -</span>{\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; b</span>}}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; +</span>}$

即可计算出姿态角[f q y]^T。Step 9: By Pose Quaternion

The attitude angle [f q y] ^T can be calculated.

The comprehensive performance of the present invention is verified below by static experiment and flight test.

1) Static experiment

In the static test, the AHRS-400CC inertial measurement unit (Inertial Measurement Units, IMUs) produced by Crossbow Technology was used to assist in the algorithm test. AHRS-400CC has a three-axis accelerometer, an angular rate gyro and a magnetometer. Through the built-in attitude estimation algorithm, it can output three Euler angles of roll, pitch and yaw. In this experiment, the gyroscope and accelerometer data collected by AHRS-400CC are used for attitude calculation, and the calculated attitude angle is compared with the "real attitude" output by AHRS-400CC. Because the attitude algorithm proposed in this paper uses the heading information provided by GPS to correct the yaw angle, and AHRS-400CC does not have a GPS sensor, so only the pitch angle and roll angle are verified in the static experiment. Place the AHRS-400CC horizontally and statically, collect sensor data and attitude angle information, and the experiment time is 160s.

Figure 2 and Figure 3 show the solution results of the attitude angle under static conditions. The solid line is the attitude angle obtained by the Kalman filter algorithm, and the dashed line is the attitude angle obtained by gyro integration. It can be seen that due to the existence of measurement noise, the attitude angle obtained by simply using the angular rate gyro measurement value integration will drift with time. The drift caused by the roll angle (about 0.0156°/s) is greater than the drift of the pitch angle (about 0.0031°/s), which is caused by the drift of the gyro in the x-axis direction of the body is greater than that in the z-axis direction. The attitude angle estimated by the Kalman filter algorithm does not drift and has a small oscillation amplitude. Table 1 shows the root mean square error (Root Mean Square Error, RMSE) comparison of the three algorithms. It can be seen that after the correction of the accelerometer, the attitude angle estimated by the Kalman filter algorithm can effectively suppress the gyro drift.

Table 1 RMSE comparison of attitude angle estimation algorithms

The estimation of angular rate gyro drift b _p , b _q is shown in Fig. 4. The solid line is b _p , and the dashed line is b _q . It can be seen from the figure that the drift of the roll rate gyro is slightly larger than that of the pitch rate gyro, which is consistent with the results shown in Figure 2 and Figure 3. Gyro drift eventually stabilizes at a constant value.

Figures 5 and 6 show the roll angle error (solid line) δf, roll rate gyro drift error (solid line) δb _p and the corresponding 1σ standard deviation bound (dashed line) obtained by the Kalman filter algorithm, respectively. As with the previous analysis, the two figures show that the error of the Kalman filter system is bounded. The 1σ standard deviation of the steady-state roll angle error is smaller than the noise standard deviation of the correction system (0.2636°), which proves once again that the attitude angle estimation using the sensor fusion algorithm is superior to the pure angular rate gyro system or the correction system. The pitch angle error is similar to the roll angle error and will not be repeated here.

同真实滚转角f之间的误差e_φ如图7所示。虚线为仅用角速率陀螺估计产生的e_φ，虽然噪声很小，但是漂移较大。实线为卡尔曼滤波算法估计产生的e_φ，可见，由传感器融合技术估计的姿态角结合了陀螺系统低噪声的校正系统不漂移的优点。estimated roll angle

The error e _φ between the true roll angle f is shown in Figure 7. The dotted line is the e _φ generated only by angular rate gyro estimation, although the noise is small, but the drift is large. The solid line is the e _φ estimated by the Kalman filter algorithm. It can be seen that the attitude angle estimated by the sensor fusion technology combines the advantages of the low-noise correction system of the gyro system without drift.

2) Flight test

In order to further verify the performance of the attitude system in the real flight environment, after the mathematical simulation and the ground experiment, a flight test is required. The test process is as follows: the commercial inertial measurement unit AHRS-400CC is placed on the carrier UAV, and the flight data is recorded after the flight starts, including three-axis acceleration, three-axis angular velocity and flight time. In order to verify the attitude estimation ability of the heading attitude system in the state of carrier maneuvering, the operator controls the aircraft to perform a large maneuvering flight. The roll angle f changes within ±80°, and the pitch angle q varies within ±40°. After the flight, use the attitude estimation algorithm proposed in this chapter to perform offline filtering calculation and analysis on the data collected by AHRS-400CC to obtain attitude angle information. Finally, compare and analyze the attitude angle obtained by filtering with the attitude angle calculated by the built-in algorithm of AHRS-400CC. Since the AHRS-400CC attitude estimation algorithm has a solution to the carrier maneuvering condition, the attitude angle output by it is considered to be the real aircraft attitude.

According to the conclusion of the simulation analysis, the overload compensation method is not available under large dynamic flight conditions, so this section only uses the time window method to process flight data. Figures 8 and 9 show the comparison between the estimated attitude angle and the true attitude angle. Three different attitude angle estimation methods were used for comparison, including time window method (dotted line), gyro integration (dashed line), and conventional Karl Mann filtering (dashed line). Since the GPS module was not used in this flight test, the yaw angle y cannot be corrected, so only the roll angle f and the pitch angle q of the UAV are estimated here.

It can be seen from Fig. 8(a) and Fig. 9(a) that the attitude angle estimated by the time window method can be well in line with the real value. In Figure (b), the attitude angle obtained by gyro integration will have a certain drift. However, the attitude angle calculated by the conventional Kalman filter is severely distorted. This is because in the flight test, the UAV is in an overload state most of the time, and the measurement data of the accelerometer can no longer reflect the attitude information faithfully, so that it cannot be correct. Correct the gyro integral value accordingly. Table 2 presents the RMSE of the three methods for pose estimation. It can be seen that the time window method can better solve the problem of attitude estimation under the condition of carrier maneuvering.

Table 2 RMSE comparison of attitude estimation based on flight test

Figure 10 shows the flag quantity controlling the filter switch in the attitude estimation process, and Figure 11 shows the trigger quantity and threshold value of the filter switch (shown by label 1 in the figure), where the trigger quantity is taken as the yaw angular velocity r . It can be seen that due to the severe yaw motion, a large yaw rate is caused. When the trigger value is greater than the threshold value, the flag value is zero, and the filter is disconnected. At this time, the attitude is estimated entirely by the angular rate gyro. When the trigger value is lower than the threshold value, the flag value is set to 1, and the Kalman filter is started. At this time, since the maneuvering overload is small, the accelerometer can be used to correct the estimated value, so as to complete the attitude determination.

In summary, the present invention proposes an attitude and heading estimation algorithm based on low-cost sensors (angular rate gyroscope, accelerometer, GPS module). Firstly, the mathematical model of the moving carrier attitude and heading reference system based on the error quaternion is derived, and the sensor fusion technology based on Kalman filter is used to improve the accuracy of the heading and attitude system. For a small UAV moving in space, when it is in a maneuvering condition, the correction system cannot accurately estimate the attitude due to the existence of additional acceleration. For this problem, two solutions, the time window method and the overload compensation method, are proposed. After determining the scheme of the attitude and heading reference system, static experiments and flight tests were carried out to verify the correctness and feasibility of the attitude algorithm. The result analysis shows that the present invention can accurately estimate the spatial attitude of the UAV during flight, and can be applied to low-cost small UAV systems.

Claims (6)

1. a kind of attitude heading reference system for low-cost small unmanned aerial vehicle, it is characterized in that, comprises angular rate gyroscope, accelerometer, GPS, angular rate gyroscope operation module, correction module and Kalman filter, wherein: The angular rate gyro measures the roll rate, pitch rate and yaw rate of the aircraft; The accelerometer measures the component of gravity under the coordinate axis of the body; GPS measures the track azimuth of the aircraft; The angular rate gyro calculation module obtains the roll angle f ₁ , the pitch angle q ₁ and the yaw angle y ₁ with offset through the roll angular rate, pitch angular rate and yaw angular rate measured by the angular rate gyro; The correction module obtains the roll angle f ₂ and the pitch angle q ₂ estimated by the correction module through the component of gravity measured by the accelerometer under the body coordinate axis system, and uses the track azimuth angle measured by the GPS as the estimated yaw angle y ₂ ; The Kalman filter fuses the data generated by the angular rate gyro operation module and the correction module to obtain the final attitude angle [f q y] ^T . 2. attitude heading reference system according to claim 1, is characterized in that, described angular rate gyro operation module uses quaternion method modeling, first defines attitude quaternion q: $\mathrm{<span\; class="notranslate">\; q</span>}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}\\ \stackrel{\mathrm{<span\; class="notranslate">\; r</span>}}{\mathrm{<span\; class="notranslate">\; q</span>}}\end{array}\right]$ where q ₀ is called the scalar part of the quaternion q, The vector part called the quaternion q, the quaternion is updated in real time by the measurement of the angular rate gyro:

Among them, p is the roll rate, q is the pitch rate, and r is the yaw rate; Then the carrier attitude angle is obtained by the following formula ${\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; arctan</span>}\frac{\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; )</span>}{\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span>}$ q ₁ =arcsin2(-q ₁ q ₃ +q ₂ q ₀ ). ${\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; arctan</span>}\frac{\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 0</span>}}<span\; class="notranslate">\; )</span>}{\mathrm{<span\; class="notranslate">\; 1</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 2</span>}<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 3</span>}}^{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; )</span>}$ 3. The attitude and heading reference system according to claim 1 or 2, wherein the correction module determines the estimated roll angle f ₂ and pitch angle q ₂ by the following formula: ${\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; arcsin</span>}\frac{{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}}{\mathrm{<span\; class="notranslate">\; g</span>}}$ ${\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; arcsin</span>}\frac{<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}}}{\mathrm{<span\; class="notranslate">\; g</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; q</span>}}$ Among them, g is the gravitational acceleration constant, and f _x is the accelerometer reading. 4. attitude heading reference system according to claim 1 or 2, is characterized in that, the specific method that described Kalman filter carries out data fusion is: Step 1: Calculate the attitude quaternion using the angular rate gyro measurement data The second step: according to formula error! Reference source not found. and forecast state error covariance matrix P ${\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{<span\; class="notranslate">\; +</span>}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; G</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; Q</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; G</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}$ In the formula, Q _k-1 is the process noise covariance matrix, and F and G are Jacobian matrices. Step 3: In the correction system, use the accelerometer and GPS to obtain the attitude angle information [f ₂ q ₂ y ₂ ] ^T estimated by the correction system. Step 4: Use the system attitude angle information estimated by the previous time step

Make difference with the attitude angle information obtained by the correction system to obtain the observations of the Kalman filter $\mathrm{<span\; class="notranslate">\; Z</span>}<span\; class="notranslate">\; =</span>\left[\begin{array}{c}\mathrm{<span\; class="notranslate">\; \&delta;\; f</span>}\\ \mathrm{<span\; class="notranslate">\; \&delta;q</span>}\\ \mathrm{<span\; class="notranslate">\; \&delta;y</span>}\end{array}\right]<span\; class="notranslate">\; =</span>\left[\begin{array}{c}{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}\\ {\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}\\ {\mathrm{<span\; class="notranslate">\; the\; y</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}\end{array}\right]<span\; class="notranslate">\; -</span>\left[\begin{array}{c}\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; f</span>}}\\ \stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; q</span>}}\\ \stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; the\; y</span>}}\end{array}\right]$ Step 5: Calculate the Kalman gain matrix ${\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; =</span>{\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}{<span\; class="notranslate">\; (</span>{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}{\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; )</span>}^{<span\; class="notranslate">\; -</span>\mathrm{<span\; class="notranslate">\; 1</span>}}$ In the formula, R _k is the measurement noise covariance matrix, and H is the Jacobian matrix; Step 6: Update the state error covariance matrix ${\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; +</span>}<span\; class="notranslate">\; =</span><span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; I</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; )</span>{\mathrm{<span\; class="notranslate">\; P</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>}{<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; I</span>}<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}{\mathrm{<span\; class="notranslate">\; h</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}<span\; class="notranslate">\; )</span>}^{\mathrm{<span\; class="notranslate">\; T</span>}}<span\; class="notranslate">\; +</span>{\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}{\mathrm{<span\; class="notranslate">\; R</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}{\mathrm{<span\; class="notranslate">\; K</span>}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{\mathrm{<span\; class="notranslate">\; T</span>}}$ Step 7: Update the state vector Step 8: Use the estimated error quaternion

and gyro drift and

Fix pose quaternion obtained in time update step

and angular rate gyro measurements

${\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; b</span>}}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; +</span>}<span\; class="notranslate">\; =</span>{\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; b</span>}}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; \&delta;</span>}{\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; b</span>}}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; +</span>}$ ${\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; +</span>}<span\; class="notranslate">\; =</span>{\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; \&omega;</span>}}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; -</span>}<span\; class="notranslate">\; -</span>{\stackrel{<span\; class="notranslate">\; ^</span>}{\mathrm{<span\; class="notranslate">\; b</span>}}}_{\mathrm{<span\; class="notranslate">\; k</span>}}^{<span\; class="notranslate">\; +</span>}$ Step 9: By Pose Quaternion

The attitude angle [f q y] ^T can be calculated. 5. The attitude and heading reference system according to claim 1 or 2, wherein when the aircraft is banked and turned, the correction module determines the estimated roll angle f ₂ and pitch angle q ₂ by the following formula ${\mathrm{<span\; class="notranslate">\; q</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; arcsin</span>}\frac{{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; x</span>}}}{\mathrm{<span\; class="notranslate">\; g</span>}}$ ${\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; 2</span>}}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; arcsin</span>}\frac{<span\; class="notranslate">\; -</span>{\mathrm{<span\; class="notranslate">\; f</span>}}_{\mathrm{<span\; class="notranslate">\; the\; y</span>}}<span\; class="notranslate">\; +</span>\mathrm{<span\; class="notranslate">\; r</span>}{\mathrm{<span\; class="notranslate">\; V</span>}}_{\mathrm{<span\; class="notranslate">\; TAS</span>}}}{\mathrm{<span\; class="notranslate">\; g</span>}\mathrm{<span\; class="notranslate">\; cos</span>}\mathrm{<span\; class="notranslate">\; q</span>}}<span\; class="notranslate">\; .</span>$ 6. The attitude heading reference system according to claim 5, characterized in that, when the aircraft is banked and turned, the norm of the angular rate is selected

As a sign to trigger the Kalman filter, the opening and closing of the time window is determined by the following formula $\left\{\begin{array}{c}\mathrm{<span\; class="notranslate">\; u</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; \&le;</span>\mathrm{<span\; class="notranslate">\; \&beta;</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; flag</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 1</span>}\\ \mathrm{<span\; class="notranslate">\; u</span>}<span\; class="notranslate">\; (</span>\mathrm{<span\; class="notranslate">\; t</span>}<span\; class="notranslate">\; )</span><span\; class="notranslate">\; ></span>\mathrm{<span\; class="notranslate">\; \&beta;</span>}<span\; class="notranslate">\; ,</span>\mathrm{<span\; class="notranslate">\; flag</span>}<span\; class="notranslate">\; =</span>\mathrm{<span\; class="notranslate">\; 0</span>}\end{array}\right.$ Among them, β is the time window threshold value, and flag is the window flag; When u(t)≤β, flag=1, the window is opened, and the Kalman filter is used for attitude determination; when u(t)>β, flag=0, the window is closed, and the filter is disconnected at this time, and the correction is stopped System fixes.

Images