Unmanned aerial vehicle attitude solving method, computing device and computer readable medium

Abstract

The application provides a UAV attitude solving method, a computing device and a computer readable medium. The UAV attitude solving method comprises: performing state one-step prediction to obtain a one-step predicted state vector at a current time; calculating a one-step predicted mean square error matrix at the current time; calculating a filtering gain at the current time based on a measurement matrix, a measurement noise covariance matrix and the one-step predicted mean square error matrix at the current time; performing state estimation based on the one-step predicted state vector at the current time, the filtering gain, a measurement vector to obtain an estimated state vector at the current time; and calculating an estimated state mean square error matrix at the current time based on the one-step predicted mean square error matrix at the current time, the filtering gain, the measurement matrix and the measurement noise covariance matrix. The state vectors all comprise a UAV attitude quaternion and a random bias of a gyroscope. Thus, the influence of the random bias can be eliminated.

Description

[Technical Field]

[0001] This invention relates to the field of unmanned aerial vehicles (UAVs), and more particularly to a method for calculating the attitude of a UAV, a computing device, and a computer-readable medium. [Background Technology]

[0002] In attitude calculation for small UAVs, low-cost MEMS (Micro-Electro-Mechanical System) sensors, including multiple sensors such as accelerometers, gyroscopes, and magnetometers, are generally used to calculate attitude. However, due to the complexity of the UAV flight environment and the large random deviation of low-cost MEMS sensors, the attitude calculation of small UAVs has a large deviation.

[0003] Therefore, there is an urgent need to propose a new technical solution to address the above problems. [Summary of the Invention]

[0004] One of the objectives of this invention is to provide a method for calculating the attitude of a UAV, which uses the attitude quaternion of the UAV and the random deviation of the gyroscope as parameters to be estimated, so as to eliminate the influence of random deviation and correct the attitude angle.

[0005] To achieve the above objectives, according to one aspect of the present invention, an attitude calculation method for an unmanned aerial vehicle (UAV) is provided. The UAV's attitude module includes a gyroscope, an accelerometer, and a magnetometer. The method includes: obtaining an estimated state vector, an estimated state mean square error matrix, and a process noise covariance matrix from the previous time step, as well as a state matrix, a measurement noise covariance matrix, measurement matrices of the accelerometer and magnetometer, and measurement vectors of the accelerometer and magnetometer from the current time step; performing a one-step state prediction based on the estimated state vector from the previous time step to obtain a one-step predicted state vector from the current time step; and calculating the one-step predicted mean square error matrix from the current time step based on the state matrix from the current time step, the estimated state mean square error matrix from the previous time step, and the process noise covariance matrix from the previous time step. The filter gain at the current moment is calculated based on the measurement matrices of the accelerometer and magnetometer at the current moment, the measurement noise covariance matrix at the current moment, and the one-step prediction mean square error matrix at the current moment. The estimated state vector at the current moment is obtained by performing state estimation based on the one-step prediction state vector at the current moment, the filter gain at the current moment, and the measurement vectors of the accelerometer and magnetometer at the current moment. The estimated state mean square error matrix at the current moment is calculated based on the one-step prediction mean square error matrix at the current moment, the filter gain at the current moment, the measurement matrices of the accelerometer and magnetometer at the current moment, and the measurement noise covariance matrix at the current moment. The estimated state vector and the one-step prediction state vector both include the attitude quaternion of the UAV and the random deviation of the gyroscope.

[0006] According to another aspect of the present invention, a computing device is provided, comprising: a memory for storing a program; and a processor for loading the program to execute the above-described UAV attitude calculation method.

[0007] According to another aspect of the present invention, a computer-readable medium is provided having a program stored therein that is executed to implement the above-described UAV attitude calculation method.

[0008] Compared with the prior art, the present invention has one or more of the following advantages: 1) Attitude calculation can be performed based on multiple sensors such as low-cost MEMS gyroscopes, accelerometers, and magnetometers; 2) The attitude quaternion of the UAV and the random deviation of the gyroscope are used as parameters to be estimated to eliminate the influence of random deviation and correct the attitude angle; 3) Under different flight conditions, based on the idea of ​​adaptive filtering, the measurement noise variance of the accelerometer and magnetometer is continuously adjusted based on the adaptive factor, which improves the robustness and anti-interference ability of attitude filtering under complex conditions. [Attached Image Description]

[0009] To more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings used in the following description of the embodiments will be briefly introduced. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0010] in:

[0011] Figure 1 This is a flowchart illustrating a method for calculating the attitude of a drone according to one embodiment of the present invention.

Detailed Implementation Methods

[0012] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the present invention will be further described in detail below with reference to the accompanying drawings and specific embodiments.

[0013] The term "an embodiment" or "embodiment" as used herein refers to a specific feature, structure, or characteristic that may be included in at least one implementation of the invention. The phrase "in one embodiment" appearing in different places throughout this specification does not necessarily refer to the same embodiment, nor is it a single or selective embodiment that excludes other embodiments. Unless otherwise specified, the terms "connected," "linked," and "connected" used herein to indicate electrical connection refer to direct or indirect electrical connection.

[0014] In this invention, unless otherwise explicitly specified and limited, the terms "connected," "linked," "coupled," etc., should be interpreted broadly; for example, they can refer to direct connection or indirect connection through an intermediate medium, which can be electronic components, functional circuits, etc. Those skilled in the art can understand the specific meaning of the above terms in this invention according to the specific circumstances.

[0015] This invention provides a method for calculating the attitude of a UAV based on adaptive extended Kalman filtering. The UAV is typically a small UAV, and its attitude module includes a low-cost attitude and heading reference system (Mems-AHRS), which includes MEMS gyroscopes, MEMS accelerometers, and MEMS magnetometers, etc.

[0016] Figure 1 This is a flowchart illustrating a UAV attitude calculation method 100 according to one embodiment of the present invention. Figure 1 As shown, the UAV attitude calculation method 100 includes the following steps.

[0017] Step 110: Obtain the estimated state vector from the previous time step. Estimate the mean square error matrix P of the state t-1 The process noise covariance matrix Q t-1 and the current state matrix Φ t / t-1 Measurement noise covariance matrix R t Measurement matrix H of accelerometer and magnetometer t The measurement vector Z of the accelerometer and magnetometer t .

[0018] In one embodiment, the estimated state vector at current time t Estimate the mean square error matrix P of the state t The process noise covariance matrix Q t At the next moment (current moment t = t + 1), it will become the estimated state vector of the previous moment t - 1. Estimate the mean square error matrix P of the state t-1 and process noise covariance matrix P t-1 t represents the current time, and t-1 represents the previous time.

[0019] Estimated state mean square error matrix P and estimated state vector Corresponding. The estimated state vector from the previous time step. Estimate the mean square error matrix P of the state t-1The estimated state vector and estimated state mean square error matrix obtained by the UAV attitude calculation method 100 in the previous moment are used in the current moment. During the initial run of the UAV attitude calculation method 100, the estimated state vector from the previous moment... It can be a predetermined state vector or a state vector obtained from gyroscope measurements, and the estimated mean square error matrix P of the state at the previous moment. t-1 This can be a predetermined mean square error matrix for the state. The estimated state vector... This includes the attitude quaternions q0, q1, q2, q3 of the UAV and the random deviations Δδx, Δδy, Δδz of the gyroscope.

[0020] 1) Based on the characteristics of MEMS gyroscopes, MEMS accelerometers, and MEMS magnetometers, this invention selects a nonlinear state-space model:

[0021]

[0022] Where X t It is an m-dimensional state vector, f(X) t )=[f1(X t f2(X) t ) … f m (X t )] T It is an m-dimensional nonlinear vector function; Z t It is an n-dimensional measurement vector, h(X) t )=[h1(X t h2(X) t ) … h n (X t )] T It is an n-dimensional nonlinear vector function; W t-1 It is the state noise vector, V t Both are measurement noise vectors, and both are sequences of Gaussian white noise vectors with zero mean, and they are uncorrelated. Q t Let R be the process noise covariance matrix. t To measure the noise covariance matrix, α tj It is the variance of the noise vector (state noise, measurement noise).

[0023] 2) Construct the state equations based on the aforementioned nonlinear state-space model. In this invention, the attitude quaternion of the UAV and the random deviation of the gyroscope are used as the state vector X at the current time t. t As shown in the following formula:

[0024] X t =[q0 t ,q1 t ,q2t ,q3 t ,Δδx t ,Δδy t ,Δδz t ] T

[0025] Thus, the state equation is:

[0026]

[0027] Among them Wq t-1 The process noise of the UAV attitude quaternion, WΔδ t-1 It is noise from the random bias of the gyroscope.

[0028] Discretize the state equations for f(X) t-1 Calculate the Jacobian matrix to obtain the state matrix at the current time step:

[0029] The state matrix Φ at the current time t / t-1 for:

[0030]

[0031] in wx represents the estimated value of the gyroscope at the current moment. t ,wy t wz t This represents the current measurement value of the gyroscope, Δδx. t ,Δδy t ,Δδz t It is the random bias of the gyroscope at the current moment, q0 t ,q1 t ,q2 t ,q3 t Let I be the attitude quaternion at the current moment. 7×7 It is a 7×7 identity matrix.

[0032] The process noise covariance matrix Q at the current time step t for:

[0033]

[0034] Where I 4×4 I is a 4×4 identity matrix. 3×3 It is a 3×3 identity matrix, α q For the noise of the attitude quaternion, α Δδ This refers to the noise caused by the random bias of the gyroscope.

[0035] 3) Construct the measurement equations based on the aforementioned nonlinear state-space model. Using the measurements from the accelerometer and magnetometer in the attitude sensor as the measurement vectors, as shown in the following equation:

[0036] Z t =[fx t ,fy t ,fz t ,mx t my t ,mz t ]

[0037] To reduce the computational burden of filtering and to minimize the number of observations with weak observability, the Z-axis measurements of the accelerometer and magnetometer are subtracted. The current measurement vectors of the accelerometer and magnetometer are then:

[0038] Z t =[fx t ,fy t ,mx t my t ]

[0039] The measurement matrix H of the accelerometer and magnetometer at the current moment t for:

[0040]

[0041] Where h11=fx t ·q0 t -fy t ·q3 t +fz t ·q2 t h12 = fx t ·q1 t +fy t ·q2 t +fz t ·q3 t ,

[0042] h13 = -fx t ·q2 t +fy t ·q1 t +fz t ·q0 t h14 = -fx t ·q3 t -fy t ·q0 t +fz t ·q1 t ,

[0043] h21=-h14 h22=-h13, h23=h12, h24=h11

[0044] h31=mx t ·q0 t -my t ·q3 t +mz t ·q2 t h32 = mx t ·q1 t +my t ·q2 t +mz t ·q3 t ,

[0045] h33 = -mx t ·q2 t +my t ·q1 t +mz t ·q0 t h34 = -mx t ·q3 t -my t ·q0 t +mz t ·q1 t

[0046] h41=-h34, h42=-h33, h43=h32, h44=h31,

[0047] Where fx t ,fy t ,fz t Let mx be the accelerometer measurement at the current time t. t my t ,mz t Let q0 be the magnetometer reading at the current time t. t ,q1 t ,q2 t ,q3 t All are attitude quaternions at the current time t.

[0048] The measurement noise covariance matrix R at the current moment t for:

[0049]

[0050] Among them, I 2×2 It is a 2×2 identity matrix, α f α represents the noise in the accelerometer measurements. m The noise of the magnetometer measurement is denoted by t, where t is the current time.

[0051] Based on equations (1) and (2), we construct an extended Kalman filter equation set.

[0052] Step 120, based on the estimated state vector from the previous time step Perform a one-step state prediction to obtain the one-step predicted state vector at the current time step.

[0053] Specifically, the one-step predicted state vector at the current time is calculated according to the following formula (3).

[0054]

[0055] The predicted state vector in the first step This also includes the drone's attitude quaternions and the random bias of the gyroscope.

[0056] Step 130, based on the current state matrix Φ t / t-1 The mean square error matrix P of the estimated state at the previous time step t-1 The process noise covariance matrix Q at the previous time step t-1 The mean square error matrix P of the one-step prediction at the current time is calculated. t / t-1 .

[0057] Specifically, the one-step prediction mean square error matrix P at the current time is calculated according to the following formula (4). t / t-1 :

[0058]

[0059] Step 140, based on the measurement matrix H of the accelerometer and magnetometer at the current moment. t The measurement noise covariance matrix R at the current moment t The mean square error matrix P of the one-step prediction at the current time. t / t-1 Calculate the filter gain K at the current time. t .

[0060] Specifically, the filter gain K at the current moment is calculated according to the following formula (5). t :

[0061]

[0062] Step 150: Predict the state vector in one step based on the current time. Current filter gain K t The measurement vector Z of the accelerometer and magnetometer at the current moment. t Perform state estimation to obtain the estimated state vector at the current time step.

[0063] Specifically, the estimated state vector at the current time is obtained by performing state estimation according to the following formula (6).

[0064]

[0065] The estimated state vector This also includes the drone's attitude quaternions and the random bias of the gyroscope.

[0066] Step 160, based on the one-step prediction mean square error matrix P at the current time. t / t-1 The current filter gain K t The measurement matrix H of the accelerometer and magnetometer at the current moment. t The measurement noise covariance matrix R at the current moment t The mean square error matrix P of the estimated state at the current time is calculated. t .

[0067]

[0068] The estimated state vector and the one-step predicted state vector both include the UAV's attitude quaternion and the gyroscope's random bias. The estimated state mean square error matrix P t With the estimated state vector Correspondingly, it is the covariance matrix of the optimal estimated state vector of the UAV.

[0069] Step 170: Calculate the attitude angle of the UAV based on the attitude quaternion of the UAV in the estimated state vector at the current moment.

[0070] This invention relates to a method for attitude calculation of small unmanned aerial vehicles based on adaptive extended Kalman filtering.

[0071] To address the 3D attitude calculation of small unmanned aerial vehicles (UAVs) during complex flight processes, an adaptive Kalman filter algorithm based on the fusion of low-cost MEMS gyroscopes, accelerometers, and magnetometers is proposed. Considering the large random bias of low-cost attitude and heading reference systems (Mems-AHRS), the UAV's attitude quaternions and the random bias of the gyroscope sensors are used as parameters to be estimated, eliminating the influence of sensor random bias and correcting the attitude angle. Furthermore, considering the influence of three-axis accelerometers and three-axis magnetometers on the attitude calculation of small UAVs under different flight conditions, an adaptive factor is proposed based on the adaptive filtering concept to continuously adjust the measurement noise variance of the accelerometer and magnetometer, improving the robustness of the attitude filtering under complex conditions. Experimental results show that the proposed algorithm not only effectively improves the attitude calculation accuracy of the nonlinear attitude model but also eliminates the influence of random bias of the attitude sensors and the measurement noise of the three-axis accelerometer and magnetometer on the attitude calculation, improving the robustness and disturbance rejection of the attitude calculation algorithm.

[0072] In a preferred embodiment, the accelerometer in the attitude sensor is significantly affected by linear acceleration and vibration, while the magnetometer is easily affected by the surrounding environment, such as magnetic field interference from high-voltage lines and magnets, which severely affects the divergence of the extended Kalman filter and ultimately the attitude calculation of the small UAV. Therefore, the measurement noise variance of the accelerometer and magnetometer can be continuously adjusted based on an adaptive factor, i.e., the measurement noise covariance matrix R at the current moment. t Replaced with:

[0073]

[0074] Where θ is the adaptive factor, R min ,R max These are the maximum and minimum values ​​of the measurement noise covariance matrix, respectively.

[0075] Through the above processing, the measurement noise covariance can be consistently limited to the interval [R]. min R max This results in better adaptive capability and filtering reliability, improving the robustness of attitude filtering under complex conditions.

[0076] In the UAV attitude calculation of this invention, considering the problem of large random deviations in low-cost attitude sensors, an adaptive extended Kalman filter method is adopted. The UAV attitude quaternion and the random deviation of the gyroscope are used as parameters to be estimated, eliminating the influence of sensor random deviations. Considering the influence of accelerometers and magnetometers in the attitude sensors on UAV attitude calculation under different flight conditions, an adaptive filtering approach is used to continuously adjust the measurement noise variance of the accelerometers and magnetometers based on adaptive factors, improving the robustness of the attitude filter under complex conditions. Experimental results show that the UAV attitude calculation not only effectively improves the attitude calculation accuracy of the nonlinear attitude model, but also eliminates the influence of random deviations of the attitude sensors and the measurement noise of the three-axis accelerometer and three-axis magnetometer on the attitude calculation, improving the robustness and disturbance rejection of the attitude calculation algorithm.

[0077] According to another aspect of the present invention, a computing device is provided, comprising: a memory for storing a program; and a processor for loading the program to execute the above-described UAV attitude calculation method 100.

[0078] According to another aspect of the present invention, a computer-readable medium is provided having a program stored therein that is executed to implement the above-described UAV attitude calculation method 100.

[0079] In the description of this specification, the references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. In addition, those skilled in the art can combine and integrate the different embodiments or examples described in this specification.

[0080] Although embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those skilled in the art can make changes, modifications and variations to the above embodiments within the scope of the present invention.

Claims

1. A method for calculating the attitude of a UAV, wherein the attitude module of the UAV includes a gyroscope, an accelerometer, and a magnetometer, characterized in that, It includes: Obtain the estimated state vector, estimated state mean square error matrix, and process noise covariance matrix of the previous time step, as well as the state matrix, measurement noise covariance matrix, measurement matrices of the accelerometer and magnetometer, and measurement vectors of the accelerometer and magnetometer at the current time step. Based on the estimated state vector of the previous time step, a one-step state prediction is performed to obtain the one-step predicted state vector of the current time step. The one-step prediction mean square error matrix at the current time is calculated based on the state matrix at the current time, the estimated state mean square error matrix at the previous time, and the process noise covariance matrix at the previous time. The filter gain at the current moment is calculated based on the measurement matrices of the accelerometer and magnetometer at the current moment, the measurement noise covariance matrix at the current moment, and the one-step prediction mean square error matrix at the current moment. The estimated state vector at the current moment is obtained by estimating the state based on the one-step predicted state vector at the current moment, the filter gain at the current moment, and the measurement vectors of the accelerometer and magnetometer at the current moment. The estimated state mean square error matrix at the current time is calculated based on the one-step prediction mean square error matrix at the current time, the filter gain at the current time, the measurement matrices of the accelerometer and magnetometer at the current time, and the measurement noise covariance matrix at the current time. The estimated state vector and the one-step predicted state vector both include the UAV's attitude quaternion and the random bias of the gyroscope. The estimated state vector, estimated state mean square error matrix, and process noise covariance matrix at the current moment will become the estimated state vector, estimated state mean square error matrix, and process noise covariance matrix at the next moment. The estimated state vector at the previous moment is: The mean square error matrix of the estimated state at the previous time step is: t-1 is the previous time step. Current state matrix for: in , This represents the estimated value of the gyroscope at the current moment. This represents the current measurement value of the gyroscope. It is the random deviation of the gyroscope at the current moment. Let be the attitude quaternion at the current moment. for The identity matrix, The process noise covariance matrix at the current time for: in for The identity matrix, for The identity matrix, For the noise of attitude quaternions, This refers to the noise caused by the random bias of the gyroscope. Measurement vectors obtained by the accelerometer and magnetometer at the current moment for: Measurement matrices obtained by the accelerometer and magnetometer at the current moment for: in , , ， , ， ， ， ， ， ， ， ， ， in The accelerometer reading is the value at the current time t. The value measured by the magnetometer at the current time t is... Measurement noise covariance matrix at the current time for: in, yes The identity matrix, The noise in the accelerometer measurement. The noise of the magnetometer measurement is denoted by t, where t is the current time.

2. The UAV attitude calculation method according to claim 1, characterized in that, Based on the estimated state vector from the previous time step, according to the following formula (3) Perform a one-step state prediction to obtain the one-step predicted state vector at the current time step. : (3) Based on the state matrix at the current time, according to the following formula (4)... The mean square error matrix of the estimated state at the previous time step The process noise covariance matrix of the previous time step The mean square error matrix of the one-step prediction at the current time is calculated. : (4) Based on the measurement matrices of the accelerometer and magnetometer at the current moment, according to the following formula (5) The measurement noise covariance matrix at the current moment The mean square error matrix of the one-step prediction at the current time. Calculate the filter gain at the current time. : (5) Based on the following formula (6), the state vector is predicted in one step at the current time. Filter gain at the current moment The measurement vectors of the accelerometer and magnetometer at the current moment. Perform state estimation to obtain the estimated state vector at the current time step. : (6) Based on the mean square error matrix of the one-step prediction at the current time, according to the following formula (7) Filter gain at the current moment The measurement matrix of the accelerometer and magnetometer at the current moment. The measurement noise covariance matrix at the current moment The mean square error matrix of the estimated state at the current time is calculated. : (7)。 3. The UAV attitude calculation method according to claim 2, characterized in that, Measurement noise covariance matrix at the current time Replaced with: in As an adaptive factor, These are the maximum and minimum values ​​of the measurement noise covariance matrix, respectively. 。 4. The UAV attitude calculation method according to claim 1, characterized in that, It also includes: The attitude angle of the UAV is calculated based on the attitude quaternion of the UAV in the estimated state vector at the current moment.

5. The UAV attitude calculation method according to claim 1, characterized in that, The gyroscope, the accelerometer, and the magnetometer are respectively a MEMS gyroscope, a MEMS accelerometer, and a MEMS magnetometer.

6. A computing device, characterized in that, include: Memory, used to store programs; A processor for loading the program to execute the UAV attitude calculation method as described in any one of claims 1-5.

7. A computer-readable medium storing a program that is executed to implement the UAV attitude calculation method as described in any one of claims 1-5.

Images