阅读说明：本技术 一种多传感器融合的无人机复杂天候飞行控制方法 (Multi-sensor fusion unmanned aerial vehicle complex weather flight control method ) 是由 李道春 姚卓尔 邵浩原 阚梓 申童 向锦武 于 2021-10-27 设计创作，主要内容包括：本发明公开了一种多传感器融合的无人机复杂天候飞行控制方法,所方法包括：建立无人机运动学方程及多个传感器的观测模型,根据观测模型及无人机运动学方程,建立无人机高度状态方程和量测方程,进一步通过卡尔曼滤波法得到多个传感器高度数据融合后的无人机飞行高度值,并根据所得飞行高度值调控无人机飞行高度。本发明可在复杂天候下获得准确的无人机高度信息,使无人机能够在复杂天候环境下进行安全稳定飞行。(The invention discloses a multi-sensor integrated unmanned aerial vehicle complex weather flight control method, which comprises the following steps: an unmanned aerial vehicle kinematic equation and observation models of a plurality of sensors are established, an unmanned aerial vehicle altitude state equation and a measurement equation are established according to the observation models and the unmanned aerial vehicle kinematic equation, the unmanned aerial vehicle flight height value after the height data of the plurality of sensors are fused is further obtained through a Kalman filtering method, and the unmanned aerial vehicle flight height is regulated according to the obtained flight height value. The invention can obtain accurate height information of the unmanned aerial vehicle in complex weather, so that the unmanned aerial vehicle can safely and stably fly in the complex weather environment.)

1. The utility model provides a multisensor fuses unmanned aerial vehicle complicated weather flight control method which characterized in that includes:

s1, establishing an unmanned aerial vehicle kinematic equation according to the conversion relation between the ground shafting and the body shafting;

s2, establishing an observation model of a plurality of sensors related to the height value in the unmanned aerial vehicle;

s3, establishing an unmanned aerial vehicle height state equation and a measurement equation according to the observation model and the unmanned aerial vehicle kinematics equation;

s4, obtaining the flying height value of the unmanned aerial vehicle after the height data of the multiple sensors are fused by a Kalman filtering method based on the height state equation and the measurement equation of the unmanned aerial vehicle;

s5, an unmanned aerial vehicle height control loop is constructed, and the flying height of the unmanned aerial vehicle is regulated and controlled according to the flying height value after the obtained data are fused.

2. The flight control method of claim 1, wherein the observation models include an atmospheric data computer observation model, a radio altimeter observation model, and a differential GPS observation model.

3. The flight control method of claim 1, wherein the drone kinematics equation is established as follows:

u＝V₀cosαcosβ

v＝V₀sinβ

w＝V₀cosβsinα

wherein the earth axis is Ox_Ey_Ez_E，The unmanned aerial vehicle speed along the x axis, the y axis and the z axis under the earth axis system, and u, v and w are components of the unmanned aerial vehicle speed along the x axis, the y axis and the z axis respectively; theta, phi and psi are respectively a pitch angle, a roll angle and a yaw angle; v₀And alpha and beta are the speed of the unmanned aerial vehicle, the attack angle of the unmanned aerial vehicle and the sideslip angle of the unmanned aerial vehicle respectively.

4. The flight control method of claim 3, wherein the observation model comprises:

the following computer observation model of atmospheric data:

h₁＝h+b_h+v₁，

wherein h is₁Height value measured for air data computer, h is real height of unmanned aerial vehicle, b_hComputer measurement of the atmospheric data for the deviation of the altitude constant, v₁Computer observed variance for atmospheric data;

the following radio altimeter observation model:

h₂＝h+v₂，

wherein h is₂Altitude value, v, measured for radio altimeter₂Is the observed variance of the radio altimeter;

the following observation model of differential GPS:

h₃＝h+v₃，

wherein h is₃Height value, v, for differential GPS measurements₃Is the observed variance of the differential GPS.

5. The flight control method of claim 4, wherein the state equation is as follows:

X＝[h b_h]^T

the measurement equation is as follows:

Z＝[h₁ h₂ h₃]^Tand, and:

wherein, w₁、w₂Is the system noise.

6. The flight control method according to claim 5, wherein in the Kalman filtering method, the time update equation of a Kalman filter is:

P_(k+1|k)＝P_(k|k)+Q_(k|k)，

and/or the presence of a gas in the gas,

the measurement update equation is:

P_(k+1)＝(I-K_(k+1)C_(k+1))P_(k+1|k)

wherein:

wherein, K_(k+1)Is the Kalman gain; c_(k+1)Outputting a matrix for the observation; p_(k+1)To estimate a mean square error matrix; q_(k+1)A covariance matrix which is the system noise; r_(k+1)Is a covariance matrix of the measured noise.

7. The flight control method of claim 1, wherein the altitude control loop employs PID control.

8. The flight control method according to claim 7, wherein the PID control takes a desired flight altitude as an input and takes the data-fused flight altitude value as a feedback.

Technical Field

The invention relates to the technical field of unmanned aerial vehicle flight control systems.

Background

The hot tide of developing unmanned aerial vehicle has been launched in the world at present, no matter be for military use field, still civilian field, unmanned aerial vehicle's position is more and more important, and the range of application is also more and more extensive. However, many problems which are not favorable for safe navigation can occur in the existing unmanned aerial vehicle under the condition of complex weather, for example, the aerodynamic performance of each control surface, wing and empennage of the unmanned aerial vehicle can be influenced under the condition of rainfall, and the flight stability and maneuverability of the unmanned aerial vehicle can be deteriorated; the appearance of the unmanned aerial vehicle, particularly the wings, can be changed under the icing condition, which can even cause the sudden reduction of the lifting force of the wings, and cause great safety accidents.

At present, when the weather condition of the unmanned aerial vehicle is complex, correct position information cannot be obtained due to interference of various factors. For example, the high phenomenon that can appear falling of unmanned aerial vehicle under influence such as rainfall, gust, nevertheless because the weather influences, the unmanned aerial vehicle altitude information that single sensor feedback obtained exists inaccurately, the great condition of error to just can influence the altitude control who corresponds unmanned aerial vehicle. Therefore, the research on the flight control of the unmanned aerial vehicle under the complex weather condition has important significance for the application development of the unmanned aerial vehicle.

Disclosure of Invention

The invention aims to provide an unmanned aerial vehicle flight control method capable of stably controlling the height under the complex weather condition, aiming at the defects that the existing unmanned aerial vehicle cannot obtain correct height information under the complex weather condition, so that the unmanned aerial vehicle has the problem of difficult flight control under the complex weather condition and the like.

The technical scheme of the invention is as follows:

a multi-sensor integrated unmanned aerial vehicle complex weather flight control method comprises the following steps:

s1, establishing an unmanned aerial vehicle kinematic equation according to the conversion relation between the ground shafting and the body shafting;

s2, establishing an observation model of a plurality of sensors related to the height value in the unmanned aerial vehicle;

s3, establishing an unmanned aerial vehicle height state equation and a measurement equation according to the observation model and the unmanned aerial vehicle kinematics equation;

s4, obtaining the flying height value of the unmanned aerial vehicle after the height data of the multiple sensors are fused by a Kalman filtering method based on the height state equation and the measurement equation of the unmanned aerial vehicle;

s5, an unmanned aerial vehicle height control loop is constructed, and the flying height of the unmanned aerial vehicle is regulated and controlled according to the flying height value after the obtained data are fused.

According to some preferred embodiments of the invention, the observation models include an atmospheric data computer observation model, a radio altimeter observation model, and a differential GPS observation model.

According to some preferred embodiments of the invention, the equations for kinematics of the drone are established as follows:

u＝V₀ cosαcosβ

v＝V₀ sinβ

w＝V₀ cosβsinα

wherein the earth axis is Ox_Ey_Ez_E，The unmanned aerial vehicle speed along the x axis, the y axis and the z axis under the ground axis system, and u, v and w are components of the unmanned aerial vehicle speed along the x axis, the y axis and the z axis of the body axis system respectively; theta, phi and psi are respectively a pitch angle, a roll angle and a yaw angle; v₀And alpha and beta are the speed of the unmanned aerial vehicle, the attack angle of the unmanned aerial vehicle and the sideslip angle of the unmanned aerial vehicle respectively.

According to some preferred embodiments of the invention, the observation model comprises:

the following computer observation model of atmospheric data:

h₁＝h+b_h+v₁，

wherein h is₁Height value measured for air data computer, h is real height of unmanned aerial vehicle, b_hComputer measurement of the atmospheric data for the deviation of the altitude constant, v₁Computer observed variance for atmospheric data;

the following radio altimeter observation model:

h₂＝h+v₂，

wherein h is₂Altitude value, v, measured for radio altimeter₂Is the observed variance of the radio altimeter;

the following observation model of differential GPS:

h₃＝h+v₃，

wherein h is₃Height value, v, for differential GPS measurements₃Is the observed variance of the differential GPS.

According to some preferred embodiments of the invention, the state equation is as follows:

X＝[h b_h]^T

the measurement equation is as follows:

Z＝[h₁ h₂ h₃]^Tand, and:

wherein, w₁、w₂Is the system noise.

According to some preferred embodiments of the present invention, in the kalman filtering method, the time update equation of the kalman filter is:

P_(k+1|k)＝P_(k|k)+Q_(k|k)，

and/or the presence of a gas in the gas,

the measurement update equation is:

P_(k+1)＝(I-K_(k+1)C_(k+1))P_(k+1|k)

wherein:

wherein, K_(k+1)Is the Kalman gain; c_(k+1)Outputting a matrix for the observation; p_(k+1)To estimate a mean square error matrix; q_(k+1)A covariance matrix which is the system noise; r_(k+1)Is a covariance matrix of the measured noise.

According to some preferred embodiments of the invention, the altitude control loop employs PID control.

According to some preferred embodiments of the present invention, the PID control uses a desired flying height as an input and uses the data-fused flying height value as a feedback.

The invention has the following beneficial effects:

according to the multi-sensor-fused unmanned aerial vehicle complex weather flight control method provided by the invention, firstly, the height data measured by the multi-sensors are fused through a Kalman filtering method, so that the real flight height of the unmanned aerial vehicle under the complex weather condition is obtained, and then the height of the unmanned aerial vehicle is controlled through a control system, so that the unmanned aerial vehicle can safely and stably fly under the complex weather environment. Compared with the existing method, the method provided by the invention has the advantages that the height of the unmanned aerial vehicle can be measured by multiple sensors under the condition of complex weather, not only single data, but also the problem of inaccurate height measurement caused by the complex weather can be solved through data fusion, reliable flying height data of the unmanned aerial vehicle can be obtained, then the unmanned aerial vehicle can be controlled to reach the expected flying height through a control system, and reference is provided for safe flying of the unmanned aerial vehicle under the condition of complex weather.

Drawings

Fig. 1 is a flow chart of a specific method for controlling the flight of an unmanned aerial vehicle in a complex weather.

Fig. 2 is a specific drone altitude control loop.

FIG. 3 is a computer measured result comparison curve of real altitude and atmospheric data in the example.

Fig. 4 is a comparison of real altitude versus radio altimeter measurements in an example.

FIG. 5 is a plot of true altitude versus differential GPS measurements for an example embodiment.

FIG. 6 is a comparison curve of the fusion result of the real height and the multi-sensor data in the embodiment.

Detailed Description

The present invention is described in detail below with reference to the following embodiments and the attached drawings, but it should be understood that the embodiments and the attached drawings are only used for the illustrative description of the present invention and do not limit the protection scope of the present invention in any way. All reasonable variations and combinations that fall within the spirit of the invention are intended to be within the scope of the invention.

Referring to fig. 1, taking the flying situation of a certain type of unmanned aerial vehicle in a complex climate as an example, a multi-sensor integrated unmanned aerial vehicle complex climate flying control method includes the following steps:

firstly, establishing an unmanned aerial vehicle kinematics equation according to a conversion relation between a ground shafting and a machine body shafting, wherein the equation is as follows:

u＝V₀cosαcosβ

v＝V₀sinβ

w＝V₀cosβsinα

wherein the earth axis is Ox_Ey_Ez_EIs a coordinate system fixed at the center of the earth and rotating along with the earth, and has an origin O_ENamely the center of the earth; defining body axis system Ox_By_Bz_BA coordinate system fixed on the unmanned aerial vehicle, an origin O_BGet the center of mass of the unmanned plane, Ox_BParallel to the longitudinal centre line of the aircraft and pointing in the direction of motion, Oy_BPerpendicular to Ox_Bz_BPlane and pointing to the right side of the aircraft, Oz_BLocated below the plane of symmetry, constituting a right-hand system, Z_EThe longitudinal displacement of the unmanned plane, u, v and w are components of the speed of the unmanned plane along an x axis, a y axis and a z axis respectively; theta, phi and psi are respectively a pitch angle, a roll angle and a yaw angle; v₀And alpha and beta are the speed of the unmanned aerial vehicle, the attack angle of the unmanned aerial vehicle and the sideslip angle of the unmanned aerial vehicle respectively.

And secondly, establishing an observation model of each height sensor in the unmanned aerial vehicle, wherein the observation model comprises: the system comprises an atmospheric data computer observation model, a radio altimeter observation model and a differential GPS observation model;

more specifically, each observation model may include:

observation model of atmospheric data computer:

h₁＝h+b_h+v₁

wherein h is₁Height value measured for air data computer, h is real height of unmanned aerial vehicle, b_hComputer measurement of the atmospheric data for the deviation of the altitude constant, v₁The variance was computer observed for atmospheric data.

Observation model of radio altimeter:

h₂＝h+v₂

wherein h is₂Altitude value, v, measured for radio altimeter₂Is the observed variance of the radio altimeter.

Observation model of differential GPS:

h₃＝h+v₃

wherein h is₃Height value, v, for differential GPS measurements₃Is the observed variance of the differential GPS.

Thirdly, establishing an unmanned aerial vehicle height state equation and a measurement equation according to the observation model and the unmanned aerial vehicle kinematics equation;

the unmanned aerial vehicle altitude state equation can be specifically established as follows:

will be longitudinally displaced Z from the unmanned aerial vehicle_EIs h ═ Z_ESubstituting the real height h of the unmanned aerial vehicle into the longitudinal motion equation of the unmanned aerial vehicle to obtain:

under normal operating conditions of all three sensors, the state equation and the measurement equation of the system are as follows:

Z＝CX+V

wherein the system state X is [ h b ]_h]^TSystematic measurement Z ═ h₁ h₂ h₃]^TA is a system state matrix, C is a measurement matrix, W is a system noise matrix, V is a measurement noise matrix,

substituting the terms into one another can obtain the specific forms of a state equation and a measurement equation as follows:

fourthly, designing a Kalman filter to obtain the flight height value of the unmanned aerial vehicle under the multi-sensor data fusion;

more specifically, according to the kalman filter method, the time update equation can be obtained as follows:

P_(k+1|k)＝P_(k|k)+Q_(k|k)

the measurement update equation is:

P_(k+1)＝(I-K_(k+1)C_(k+1))P_(k+1|k)

wherein:

wherein, K_(k+1)Is the Kalman gain; c_(k+1)Observing an output matrix for the sensor; p_(k+1)To estimate a mean square error matrix; q_(k+1)A covariance matrix which is the system noise; r_(k+1)Is a covariance matrix of the measured noise.

An unbiased estimation value of the flying height of the unmanned aerial vehicle can be obtained from the flight height of the unmanned aerial vehicleThis value is very close to the true height h of the drone and can be considered equal.

And fifthly, constructing an unmanned aerial vehicle height control loop, and regulating and controlling the flying height of the unmanned aerial vehicle according to the obtained flying height value.

More specifically, referring to fig. 2, the altitude control loop may adopt a PID control method to take the expected flying altitude as input, take the obtained real altitude value of the multi-sensor data fusion as feedback, adjust the flying altitude of the unmanned aerial vehicle so that the real flying altitude of the unmanned aerial vehicle reaches the input expected flying altitude, and finally complete the flying altitude control of the unmanned aerial vehicle under the complex weather conditions.

In some embodiments, the actual altitude and air data computer measurement curve obtained according to the above embodiments is shown in fig. 3, the actual altitude and radio altimeter measurement curve is shown in fig. 4, the actual altitude and differential GPS measurement curve is shown in fig. 5, and the actual altitude and multi-sensor data fusion curve is shown in fig. 6, it can be seen that the altitude data obtained by the multi-sensor fusion method of the present invention is most consistent with the actual altitude.

The above examples are only used to illustrate some embodiments of the present invention, and the scope of the present invention is not limited to the above examples. All technical schemes belonging to the idea of the invention belong to the protection scope of the invention. It should be noted that modifications and embellishments within the scope of the invention may be made by those skilled in the art without departing from the principle of the invention, and such modifications and embellishments should also be considered as within the scope of the invention.