阅读说明：本技术 一种基于单天线gps和imu的大机动条件下无人机姿态估计方法 (Unmanned aerial vehicle attitude estimation method based on single-antenna GPS and IMU under large-mobility condition ) 是由 沈锋 卢旺 徐定杰 周威 赵雨晴 于 2018-08-02 设计创作，主要内容包括：本发明提供了一种基于低成本MEMS传感器和单天线GPS的应用于无人机在大机动条件下的姿态估计算法。该算法将GPS测得的速度值差分,得到的结果对IMU测得的线性加速度进行补偿,采用互补滤波融合算法,再利用校正后的加速度信息修正陀螺仪的漂移,从而得到当前姿态角的准确估计,并在无人机采用BTT控制飞行过程中实时利用GPS信息对偏航角进行校正,输出融合并校正的最佳姿态信息。(The invention provides an attitude estimation algorithm based on a low-cost MEMS sensor and a single-antenna GPS and applied to an unmanned aerial vehicle under a large maneuvering condition. The algorithm differentiates speed values measured by a GPS, the obtained result compensates linear acceleration measured by an IMU, a complementary filtering fusion algorithm is adopted, corrected acceleration information is used for correcting drift of a gyroscope, so that accurate estimation of the current attitude angle is obtained, the GPS information is used for correcting the yaw angle in real time in the process that the unmanned aerial vehicle controls flight by adopting the BTT, and the fused and corrected optimal attitude information is output.)

1. An unmanned aerial vehicle attitude estimation method under large maneuvering conditions based on single-antenna GPS and IMU is characterized by comprising the following steps:

step 1: and setting initial attitude quaternion and processing sensor data. Carrying out low-pass filtering on output data of the gyroscope and the accelerometer, and carrying out difference on a GPS measured value to obtain linear acceleration to compensate accelerometer data;

step 2: constructing a conversion matrix according to the attitude quaternion, and calculating a reference attitude quaternion error amount by using accelerometer data;

and step 3: designing a PI controller to complete the compensation of the accelerometer on the drift of the gyroscope;

and 4, step 4: and integrating the compensated gyroscope data, converting the yaw angle measurement value obtained by the GPS into an attitude quaternion, fusing the attitude quaternion with the quaternion obtained by the complementary filtering, and finally converting the attitude quaternion into a corresponding unmanned aerial vehicle attitude angle.

2. The method for estimating the attitude of the unmanned aerial vehicle under the condition of large maneuvering based on the single antenna GPS and IMU of claim 1, wherein the step 1 comprises the following steps:

step 1.1: selecting an attitude quaternion as a state vector:

Step 1.2: the gyroscope and accelerometer sensor data are preprocessed by a first-order low-pass filter g(s) ═ a/(s + a) to eliminate sensor interference caused by vibration. Differentiating the GPS speed measurement value by using a third-order optimal differentiatorTo linear motion acceleration a_gps＝(0.0971d₁+0.1662d₂+0.1853d₃)/2T_gps。

3. The method for estimating the attitude of the unmanned aerial vehicle under the condition of large maneuvering based on the single antenna GPS and IMU of claim 1, wherein the step 2 comprises the following steps:

step 2.1: normalizing accelerometer data

a_acc＝[a_x/norm,a_y/norm,a_z/norm]

Constructing an attitude quaternion transformation matrix

Compensation of accelerometer measurements using GPS difference results, solving the resulting rotation matrix using quaternionsSolving out an estimated value

a＝[a_xgps/norm,a_ygps/norm,(a_zgps+g)/norm]

Step 2.2: cross multiplication to obtain reference quaternion error vector

[e_xe_ye_z]^T＝[v_xv_yv_z]^T×[a_ccxa_ccya_ccz]^T

4. The method for estimating the attitude of the unmanned aerial vehicle under the condition of large maneuvering based on the single antenna GPS and IMU of claim 1, wherein the step 3 comprises the following steps:

step 3.1: expanding to obtain calculation error, and correcting gyroscope by using PI

5. The method of estimating pose of unmanned aerial vehicle under large maneuvering conditions based on single antenna GPS and IMU of claim 1, wherein step 4 comprises the steps of:

step 4.1: updating quaternion by using the corrected data of the gyroscope, and adopting a Longge Kuta integral gyro ═ g_xg_yg_z]

gyro＝gyro+δ

q₀＝q₀+(-q₁*g_x-q₂*g_y-q₃*g_z)*ΔT

q₁＝q₁+(q₀*g_z+q₂*g_z-q₃*g_y)*ΔT

q₂＝q₂+(q₀*g_y-q₁*g_z+q₃*g_x)*ΔT

q₃＝q₃+(q₀*g_z+q₁*g_y-q₂*g_x)*ΔT

Step 4.2: measuring yaw angle psi by GPS_gpsIncorporation of reference values into quaternions

q₀＝cos(φ)cos(θ)cos(ψ_gps)+sin(φ)sin(θ)sin(ψ_gps)；

q₁＝sin(φ)cos(θ)cos(ψ_gps)-cos(φ)sin(θ)sin(ψ_gps)；

q₂＝cos(φ)sin(θ)cos(ψ_gps)+sin(φ)cos(θ)sin(ψ_gps)；

q₃＝cos(φ)cos(θ)sin(ψ_gps)-sin(φ)sin(θ)cos(ψ_gps)；

Step 4.3: after quaternion normalization, the quaternion is converted into an attitude angle

roll＝arctan(2*(q₀*q₁+q₂*q₃)/(1-2(q₁*q₁+q₂*q₂)))

pitch＝arcsin(2*(q₀*q₂-q₁*q₃))

yaw＝arctan(2*(q₀*q₃+q₂*q₁)/(2(q₁*q₁+q₀*q₀)-1))。

Technical Field

The invention belongs to the technical field of unmanned aerial vehicle navigation, and particularly relates to an unmanned aerial vehicle attitude estimation method under a large-mobility condition based on a single-antenna GPS and an IMU.

Background

An unmanned plane (unmanned aerial vehicle) is an unmanned plane controlled by radio remote control and an onboard program controller. It appeared in the 20 th century for the first time, and was used as a target in military training, and gradually turned to various multipurpose fields such as investigation and attack through the continuous development of the last hundred years. Compared with a manned airplane, the unmanned airplane has the advantages of low cost, strong viability, no casualty risk, convenient use and the like, so that the unmanned airplane can play an important role in military affairs and has wide application prospect in the civil field.

Attitude estimation is a precondition for realizing attitude control of the unmanned aerial vehicle, is an integral important component of a navigation system, and directly influences the survival capability of the unmanned aerial vehicle. In traditional unmanned aerial vehicle attitude control, the attitude angle can be obtained by gyroscope integration, and can also be derived by acceleration sensor measurement gravitational acceleration in the three-axis vector decomposition coordinates of the carrier system. The gyroscope has good high-frequency dynamic response characteristics for resolving the attitude angle, and the output of the gyroscope can quickly respond to the change of the attitude angle; no high-frequency noise interference exists, and the output value is smooth; the output of the gyroscope is not interfered by external acceleration, and stable output can be still maintained under the condition that the carrier is in violent vibration. But it also has some defects, such as the zero point of the gyroscope will drift along with the change of temperature and other external environment factors, the integral of the output value of the gyroscope will generate accumulated error, and the calculation error is larger after long-time operation. The low-frequency characteristic of the attitude angle calculated by the accelerometer is good, the static output is stable, and no drift or accumulated error exists; the attitude calculation amount is small, and accurate instantaneous attitude angle data can be rapidly obtained under the condition of no external acceleration (only gravity acceleration). The method has the following defects that high-frequency noise interference exists, the high-frequency dynamic characteristic is poor, the output of the method cannot rapidly respond to the rapid change of the attitude angle, the method is easily interfered by external acceleration, and if acceleration except for the gravity acceleration exists, the accurate attitude angle cannot be obtained.

Disclosure of Invention

The existing attitude estimation algorithm of the unmanned aerial vehicle has the advantage that under the condition of large maneuvering, the accelerometer measurement value containing harmful linear acceleration may greatly exceed the gravity acceleration, so that bad influence is brought to the stability and accuracy of the algorithm. In addition, the geomagnetic vector is very weak, and if strong magnetic interference exists in the working environment of the unmanned aerial vehicle, useful signals can be even submerged in noise. The invention introduces the speed information measured by the GPS to compensate the accelerometer, so that the unmanned aerial vehicle can still obtain an accurate attitude estimation value under the maneuvering condition, and the GPS information is utilized to correct the yaw angle during the movement. Aiming at the defects of the prior art, the invention provides an unmanned aerial vehicle attitude estimation method based on a single-antenna GPS and an IMU under a large-mobility condition.

The technical scheme of the invention is as follows:

step 1: and setting initial attitude quaternion and processing sensor data. Carrying out low-pass filtering on output data of the gyroscope and the accelerometer, and carrying out difference on a GPS measured value to obtain linear acceleration to compensate accelerometer data;

step 2: constructing a conversion matrix according to the attitude quaternion, and calculating a reference attitude quaternion error amount by using accelerometer data;

and step 3: designing a PI controller to complete the compensation of the accelerometer on the drift of the gyroscope;

and 4, step 4: and integrating the compensated gyroscope data, converting the yaw angle measurement value obtained by the GPS into an attitude quaternion, fusing the attitude quaternion with the quaternion obtained by the complementary filtering, and finally converting the attitude quaternion into a corresponding unmanned aerial vehicle attitude angle.

2. The method for estimating the attitude of the unmanned aerial vehicle under the condition of large maneuvering based on the single antenna GPS and IMU of claim 1, wherein the step 1 comprises the following steps:

step 1.1: selecting an attitude quaternion as a state vector:

taking x as initial value ═ 1,0,0,0]；

Step 1.2: the gyroscope and accelerometer sensor data are preprocessed by a first-order low-pass filter g(s) ═ a/(s + a) to eliminate sensor interference caused by vibration. Differentiating the GPS speed measurement value by using a third-order optimal differentiator to obtain the linear motion acceleration a_gps＝(0.0971d₁+0.1662d₂+0.1853d₃)/2T_gps。

3. The method for estimating the attitude of the unmanned aerial vehicle under the condition of large maneuvering based on the single antenna GPS and IMU of claim 1, wherein the step 2 comprises the following steps:

step 2.1: normalizing accelerometer data

a_acc＝[a_x/norm,a_y/norm,a_z/norm]

Constructing an attitude quaternion transformation matrix

Compensation of accelerometer measurements using GPS difference results, solving the resulting rotation matrix using quaternions

Solving out an estimated value

And normalizing the reference acceleration vector

a＝[a_xgps/norm,a_ygps/norm,(a_zgps+g)/norm]

Step 2.2: cross multiplication to obtain reference quaternion error vector

[e_xe_ye_z]^T＝[v_xv_yv_z]^T×[a_ccxa_ccya_ccz]^T

4. The method for estimating the attitude of the unmanned aerial vehicle under the condition of large maneuvering based on the single antenna GPS and IMU of claim 1, wherein the step 3 comprises the following steps:

step 3.1: expanding to obtain calculation error, and correcting gyroscope by using PI

5. The method of estimating pose of unmanned aerial vehicle under large maneuvering conditions based on single antenna GPS and IMU of claim 1, wherein step 4 comprises the steps of:

step 4.1: updating quaternion by using the corrected data of the gyroscope by adopting the Runge Kutta integral

gyro＝[g_xg_yg_z]

gyro＝gyro+δ

q₀＝q₀+(-q₁*g_x-q₂*g_y-q₃*g_z)*ΔT

q₁＝q₁+(q₀*g_z+q₂*g_z-q₃*g_y)*ΔT

q₂＝q₂+(q₀*g_y-q₁*g_z+q₃*g_x)*ΔT

q₃＝q₃+(q₀*g_z+q₁*g_y-q₂*g_x)*ΔT

Step 4.2: measuring yaw angle psi by GPS_gpsIncorporation of reference values into quaternions

q₀＝cos(φ)cos(θ)cos(ψ_gps)+sin(φ)sin(θ)sin(ψ_gps)；

q₁＝sin(φ)cos(θ)cos(ψ_gps)-cos(φ)sin(θ)sin(ψ_gps)；

q₂＝cos(φ)sin(θ)cos(ψ_gps)+sin(φ)cos(θ)sin(ψ_gps)；

q₃＝cos(φ)cos(θ)sin(ψ_gps)-sin(φ)sin(θ)cos(ψ_gps)；

Step 4.3: after quaternion normalization, the quaternion is converted into an attitude angle

roll＝arctan(2*(q₀*q₁+q₂*q₃)/(1-2(q₁*q₁+q₂*q₂)))

pitch＝arcsin(2*(q₀*q₂-q₁*q₃))

yaw＝arctan(2*(q₀*q₃+q₂*q₁)/(2(q₁*q₁+q₀*q₀)-1))

The invention has the beneficial effects that:

(1) the invention provides an unmanned aerial vehicle attitude estimation method under large maneuvering conditions based on a single-antenna GPS and an IMU (inertial measurement Unit), which introduces speed information measured by the GPS, compensates an accelerometer after differentiation, and ensures that the unmanned aerial vehicle can still obtain an accurate attitude estimation value under the maneuvering conditions;

(2) the invention replaces the yaw angle with the included angle between the velocity vector measured by the GPS information and the due north direction, and makes up the disadvantage of the magnetometer in the magnetic interference environment;

(3) the method applies complementary filtering to IMU output data corrected by using GPS data to perform data fusion processing to obtain final attitude navigation information, and the arithmetic operation speed is high;

(4) the invention has low requirement on hardware, can realize a series of algorithms through software programming and has lower cost.

Drawings

Fig. 1 is an overall block diagram of the unmanned aerial vehicle attitude estimation method based on single-antenna GPS and IMU under large maneuvering conditions.

Fig. 2 is a schematic diagram of the PI controller of the present invention.

Fig. 3 is a schematic diagram of a coordinate system.

Detailed Description

The present invention will be described in detail below with reference to the accompanying drawings.

A method for estimating the attitude of an unmanned aerial vehicle under large-mobility conditions based on a single-antenna GPS and an IMU (inertial measurement Unit) is shown in figure 1 and comprises the following steps:

step 1: and setting initial attitude quaternion and processing sensor data. Carrying out low-pass filtering on output data of the gyroscope and the accelerometer, and carrying out difference on a GPS measured value to obtain linear acceleration compensation accelerometer data;

step 2: constructing a conversion matrix according to the attitude quaternion, and calculating a reference attitude quaternion error amount by using accelerometer data, wherein a coordinate system is defined as shown in FIG. 3;

and step 3: designing a PI controller to complete the compensation of the accelerometer on the drift of the gyroscope, wherein the principle of the PI controller is shown in FIG. 2;

and 4, step 4: and integrating the compensated gyroscope data, converting the yaw angle measurement value obtained by the GPS into an attitude quaternion, fusing the attitude quaternion with the quaternion obtained by the complementary filtering, and finally converting the attitude quaternion into a corresponding unmanned aerial vehicle attitude angle.

The step 1 specifically comprises the following steps:

step 1.1: selecting an attitude quaternion as a state vector:

taking x as initial value ═ 1,0,0,0]；

Step 1.2: the gyroscope and accelerometer sensor data are preprocessed by a first-order low-pass filter g(s) ═ a/(s + a) to eliminate sensor interference caused by vibration. Differentiating the GPS speed measurement value by using a third-order optimal differentiator to obtain the linear motion acceleration a_gps＝(0.0971d₁+0.1662d₂+0.1853d₃)/2T_gps。

The step 2 specifically comprises the following steps:

step 2.1: normalizing accelerometer data

a_acc＝[a_x/norm,a_y/norm,a_z/norm]

Constructing an attitude quaternion transformation matrix

Compensation of accelerometer measurements using GPS difference results, solving the resulting rotation matrix using quaternions

Solving out an estimated value

And normalizing the reference acceleration vector

a＝[a_xgps/norm,a_ygps/norm,(a_zgps+g)/norm]

Step 2.2: cross multiplication to obtain reference quaternion error vector

[e_xe_ye_z]^T＝[v_xv_yv_z]^T×[a_ccxa_ccya_ccz]^T

The step 3 specifically comprises the following steps:

step 3.1: expanding to obtain calculation error, and correcting gyroscope by using PI

The step 4 specifically comprises the following steps:

step 4.1: updating quaternion by using the corrected data of the gyroscope by adopting the Runge Kutta integral

gyro＝[g_xg_yg_z]

gyro＝gyro+δ

q₀＝q₀+(-q₁*g_x-q₂*g_y-q₃*g_z)*ΔT

q₁＝q₁+(q₀*g_z+q₂*g_z-q₃*g_y)*ΔT

q₂＝q₂+(q₀*g_y-q₁*g_z+q₃*g_x)*ΔT

q₃＝q₃+(q₀*g_z+q₁*g_y-q₂*g_x)*ΔT

Step 4.2: measuring yaw angle psi by GPS_gpsIncorporation of reference values into quaternions

q₀＝cos(φ)cos(θ)cos(ψ_gps)+sin(φ)sin(θ)sin(ψ_gps)；

q₁＝sin(φ)cos(θ)cos(ψ_gps)-cos(φ)sin(θ)sin(ψ_gps)；

q₂＝cos(φ)sin(θ)cos(ψ_gps)+sin(φ)cos(θ)sin(ψ_gps)；

q₃＝cos(φ)cos(θ)sin(ψ_gps)-sin(φ)sin(θ)cos(ψ_gps)；

Step 4.3: after quaternion normalization, the quaternion is converted into an attitude angle

roll＝arctan(2*(q₀*q₁+q₂*q₃)/(1-2(q₁*q₁+q₂*q₂)))

pitch＝arcsin(2*(q₀*q₂-q₁*q₃))

yaw＝arctan(2*(q₀*q₃+q₂*q₁)/(2(q₁*q₁+q₀*q₀)-1))

The invention has the following beneficial effects:

(1) the invention provides an unmanned aerial vehicle attitude estimation method under large maneuvering conditions based on a single-antenna GPS and an IMU (inertial measurement Unit), which introduces speed information measured by the GPS, compensates an accelerometer after differentiation, and ensures that the unmanned aerial vehicle can still obtain an accurate attitude estimation value under the maneuvering conditions;

(2) the invention replaces the yaw angle with the included angle between the velocity vector measured by the GPS information and the due north direction, and makes up the disadvantage of the magnetometer in the magnetic interference environment;

(3) the method applies complementary filtering to IMU output data corrected by using GPS data to perform data fusion processing to obtain final attitude navigation information, and the arithmetic operation speed is high;

(4) the invention has low requirement on hardware, can realize a series of algorithms through software programming and has lower cost.