A multi-sensor fusion method for quadrotor drone attitude estimation

Abstract

The present invention provides a multi-sensor fusion method for estimating the attitude of a quadrotor drone. The method comprises the following steps: Step S1, establishing a state observation model for the quadrotor drone; Step S2, filtering multi-channel sensor signals; Step S3, calculating linear combination parameters of the multi-channel sensor signals; Step S4, calculating sensor values ​​for a fixed axis based on the linear combination parameters; Step S5, constructing an inverse equation for the observation model and performing attitude estimation based on the observation results. The present invention has the advantages of high computation speed, good measurement stability, and fast response speed.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicles (UAVs), and in particular to a multi-sensor fusion method for estimating the attitude of a quad-rotor UAV. Background Art

[0002] Quadrotors are lightweight, flexible, simple-structured aircraft capable of vertical takeoff and landing. They are widely used in multimedia, agriculture, industry, military, and other fields. The attitude reference system (ARS) is a crucial component of quadrotor ARS control and significantly influences the control performance of the final controller. Therefore, designing a highly reliable and accurate state estimation algorithm for this ARS has significant engineering significance and application value.

[0003] Many factors affect the stability of a quadrotor's attitude control system, including system inertia uncertainty, external wind torque disturbances, and interfering torques such as gyroscopic torque caused by the rotors. Quadrotors' actuators are brushless DC motors, which can experience partial failures due to manufacturing processes and high-intensity missions. Furthermore, brushless DC motors have a maximum allowable instantaneous current. If the control signal is too large, the motor's load current is excessive, potentially burning out the motor. Existing quadrotor state estimation methods include complementary filtering, explicit complementary filtering, gradient descent, and extended Kalman filtering.

[0004] However, existing quadrotor drones have high computational performance overhead, weak noise suppression performance, and the estimation results are easily affected by a single signal, with low accuracy and poor stability. Summary of the Invention

[0005] In response to the shortcomings of the above-mentioned related technologies, the present invention proposes a multi-sensor fusion quadrotor UAV attitude estimation method to solve the problems of existing quadrotor UAVs, such as high computational performance overhead, weak noise suppression performance, estimation results being easily affected by a single signal, and low accuracy.

[0006] In order to solve the above technical problems, an embodiment of the present invention provides a multi-sensor fusion quadrotor drone attitude estimation method, which includes the following steps:

[0007] Step S1: Establish a state observation model of a quadrotor drone;

[0008] Step S2: filtering the multi-channel sensor signals;

[0009] Step S3, calculating the linear combination parameters of the multi-channel sensor signals;

[0010] Step S4, calculating the sensor value of the fixed axis according to the linear combination parameter;

[0011] Step S5: construct an inverse solution equation of the state observation model and perform posture solution based on the observation results.

[0012] Preferably, the step S1 specifically includes the following sub-steps:

[0013] Obtaining a theoretical acceleration value based on observation data of an acceleration sensor of the quadrotor drone;

[0014] Obtaining a theoretical magnetic value based on an observation value of a magnetometer of the quadrotor drone;

[0015] Establishing a determination model according to the acceleration theoretical value and the magnetic force theoretical value;

[0016] According to the model, the attitude represented by the quaternion and the acceleration provided by the pulling force of the quadrotor drone are solved.

[0017] Preferably, step S2 specifically includes the following sub-steps:

[0018] The error between the actual observed sensor data and the ideal observed data is a set of noises that obeys Gaussian distribution;

[0019] Use Kalman filter to filter out noise from data;

[0020] Within multiple channels, the data are serially filtered by using a multi-sensor optimal information fusion method.

[0021] Preferably, the step S3 specifically includes the following sub-steps:

[0022] Obtain magnetometer fusion signal data of three linearly independent channels;

[0023] Obtaining a linear combination according to the fused signal data;

[0024] The linear combination is introduced into the state observation model to obtain an ideal measurement value of the magnetic force.

[0025] Preferably, the step S4 specifically includes the following sub-steps:

[0026] Reversely obtain corresponding linear combination parameters according to the linear combination;

[0027] The linear combination parameters are applied to the state observation model to obtain a combined observation value at a moment.

[0028] Preferably, the step S5 specifically includes the following sub-steps:

[0029] Solve the attitude quaternion and the acceleration provided by the pulling force according to the state observation model;

[0030] The pulling force of the drone is obtained by calculating the acceleration provided by the pulling force.

[0031] Compared with related technologies, the present invention establishes a state observation model for a quadrotor drone; filters multi-channel sensor signals; calculates linear combination parameters of the multi-channel sensor signals; calculates fixed-axis sensor values ​​based on the linear combination parameters; constructs an inverse solution equation for the observation model, and performs attitude solution based on the observation results. In this way, an attitude analysis solution method based on the observation model is established, which changes the calculation method of state estimation and simplifies the solution process. To address the problem of insufficient noise suppression performance, a new vector generation and observation method is used to reduce the error caused by a single sensor and solve the problem of fluctuation and drift in direct observation. As the estimation results are easily affected by a single signal, an optimal information fusion method and a multi-channel filtering and fusion method are used, and multiple sets of data are used for measurement to increase the stability of the values. Therefore, the present invention has the advantages of fast computing speed, good measurement stability, and fast response speed. BRIEF DESCRIPTION OF THE DRAWINGS

[0032] The present invention will be described in detail below with reference to the accompanying drawings. The above and other aspects of the present invention will become clearer and easier to understand through the detailed description made with reference to the following drawings. In the accompanying drawings:

[0033] Figure 1 This is a flow chart of the multi-sensor fusion quadrotor drone attitude estimation method of the present invention;

[0034] Figure 2 This is the overall flow chart of the multi-sensor fusion quadrotor drone attitude estimation method of the present invention. DETAILED DESCRIPTION

[0035] The specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0036] The specific embodiments / examples described herein are specific embodiments of the present invention and are used to illustrate the concept of the present invention. They are illustrative and exemplary and should not be construed as limiting the embodiments of the present invention or the scope of the present invention. In addition to the examples described herein, those skilled in the art can also adopt other obvious technical solutions based on the claims and the disclosure of the specification. These technical solutions, including any obvious replacements and modifications of the embodiments described herein, are all within the scope of protection of the present invention.

[0037] Example 1

[0038] Please see the attached Figure 1-2 As shown, the present invention provides a multi-sensor fusion quadrotor drone attitude estimation method, the method comprising the following steps:

[0039] Step S1: Establish a state observation model for a quadrotor drone.

[0040] Step S2: filtering the multi-channel sensor signals.

[0041] Step S3: Calculate the linear combination parameters of the multi-channel sensor signals.

[0042] Step S4: Calculate the sensor value of the fixed axis according to the linear combination parameters.

[0043] Step S5: construct an inverse solution equation of the state observation model and perform posture solution based on the observation results.

[0044] Specifically, a state observation model of a quad-rotor drone is established; multi-channel sensor signals are filtered; linear combination parameters of the multi-channel sensor signals are calculated; fixed-axis sensor values ​​are calculated based on the linear combination parameters; an inverse solution equation of the state observation model is constructed, and attitude solution is performed based on the observation results. In this way, an attitude analytical solution method based on the observation model is established, the calculation method of the state estimation is changed, and the solution process is simplified; in order to address the problem of insufficient noise suppression performance, the error caused by a single sensor is reduced through a new vector generation and observation method, and the problem of fluctuation and drift in direct observation is solved; in order to address the problem that the estimation result is easily affected by a single signal, an optimal information fusion method and a multi-channel filtering and fusion method are used, and multiple sets of data are used for measurement to increase the stability of the values. Therefore, the present invention has the advantages of fast computing speed, good measurement stability, and fast response speed.

[0045] In this embodiment, step S1 specifically includes the following sub-steps:

[0046] The theoretical acceleration value is obtained based on the observation data of the acceleration sensor of the quadrotor drone.

[0047] Among them, it is known that for a quadrotor drone, the observed data of the acceleration sensor has the following theoretical values:

[0048]

[0049] Where: a body is the theoretical observation data of acceleration in body coordinates; is the rotation matrix from ground coordinates to body coordinates; f is the pulling force provided by the drone itself; g is the acceleration due to gravity.

[0050] The theoretical magnetic value is obtained according to the observation value of the magnetometer of the quadrotor drone.

[0051] Among them, the observed values ​​of the magnetometer have the following theoretical values:

[0052]

[0053] Where: M body Theoretical observation data of magnetic force in body coordinates; is the rotation matrix from ground coordinates to body coordinates; It is the earth coordinate magnetic data of x, y, and z axes in the NED coordinate system.

[0054] A determination model is established according to the acceleration theoretical value and the magnetic force theoretical value.

[0055] Among them, under the condition that the observation data is clearly known, under the condition When established, the posture described by quaternion q = [q0, q1, q2, q3] has the following determination model:

[0056]

[0057] According to the model, the attitude represented by the quaternion and the acceleration provided by the pulling force of the quadcopter are solved. Through the model, the attitude q represented by the quaternion and the acceleration a provided by the pulling force of the quadcopter can be solved. f .

[0058] In this embodiment, step S2 specifically includes the following sub-steps:

[0059] The error between the actual observed sensor data and the ideal observed data is a set of noises that obeys Gaussian distribution.

[0060] Use Kalman filter to remove noise from the data.

[0061] Within multiple channels, the data are serially filtered by using a multi-sensor optimal information fusion method.

[0062] Specifically, the error between the actual sensor data and the ideal observation data is a set of noise that follows a Gaussian distribution. A Kalman filter is used to filter out the noise from the data.

[0063] To facilitate the processing of the subsequent step S3, the data is divided into k groups according to the following requirements and filtered separately:

[0064]

[0065] Where: CH i is the signal group of channel i; is the element in the i-th group of signals, and the size of the data vector contained in each element in each group is the same; The attitude rotation matrix of the current sampling position back to the ground coordinate system; N is 1 to A natural number; i is the channel number.

[0066] Within the channel, the data is serially filtered using the multi-sensor optimal information fusion method:

[0067]

[0068] Where: Data i is the final result of channel i; d i,j is the value of the signal h in channel i; P ij is the covariance matrix of signal j in channel i; k is the number of channels.

[0069] In this embodiment, step S3 specifically includes the following sub-steps:

[0070] Acquire magnetometer fusion signal data from three linearly independent channels.

[0071] Among them, the magnetometer fusion signal of three linearly independent channels can be expressed as:

[0072] M i =Ro i M r ,i=1,2,3

[0073] Where: M i is a column vector consisting of the ideal observation values ​​of the magnetometer of channel i; Ro i M is the rotation matrix from the ground to the body based on the attitude of channel i; r is the actual magnetic force vector.

[0074] A linear combination is obtained based on the fused signal data. So that:

[0075]

[0076] The linear combination is introduced into the state observation model to obtain an ideal measurement value of the magnetic force.

[0077] Among them, a set of physical values ​​of a fixed component in a certain direction can be combined through nonlinearly related vectors. It is not difficult to find that when this measurement signal changes with posture, it has good homogeneity, and the linear combination can be introduced into the observation model:

[0078] M i,t =R t Ro i M r

[0079]

[0080] Where: M i,t is the ideal measurement value of channel i at time t; R t is the ground-to-body rotation matrix at time t.

[0081] In this way, by synthesizing the sensor data signal, the target data vector of the sensor is synthesized instead of directly using the actual magnetic field vector with declination. This not only directly reduces the influence of magnetic declination and non-uniform magnetic field on attitude measurement, but also reduces the computational complexity of the model by realizing one-dimensional observation of the rotation matrix.

[0082] In this embodiment, step S4 specifically includes the following sub-steps:

[0083] Reversely obtain corresponding linear combination parameters based on the linear combination.

[0084] The linear combination parameters are applied to the state observation model to obtain a combined observation value at a moment.

[0085] Specifically, following step S3, the corresponding linear combination parameters can be obtained in reverse:

[0086]

[0087] Where A is the expected linear combination parameter; a i is the linear combination parameter element; M i,0 is the column vector of measurement data of channel i under initial conditions; M target is the initial value of the numerical vector expected to be generated, set to the requirements of the observation model:

[0088]

[0089] For a set of measurement values ​​that have completed filtering and data fusion, the measurement value of the combined expected vector is:

[0090]

[0091] Where: M target,t is the combined observation value at time t; M i,t is the observation value of channel i at time t; A is the linear combination parameter.

[0092] In this embodiment, step S5 specifically includes the following sub-steps:

[0093] Solve the attitude quaternion and the acceleration provided by the pulling force according to the state observation model;

[0094] The pulling force of the drone is obtained by calculating the acceleration provided by the pulling force.

[0095] Specifically, the equations are sorted out, and there is an attitude solution equation. It is not difficult to conclude that we need to solve the attitude first before we can solve the tension, so there is an attitude solution equation:

[0096]

[0097] Where: M i is the fusion observation value of channel i; M x , M y , M z are the x, y, and z axis observation values ​​of the expected vector.

[0098] To solve this equation, we assume:

[0099] g 31 =a x +gM z

[0100] g 20 =a x -gM z

[0101]

[0102] Where: g 31 , g 20 It is the multiplication-related parameter of the quaternion q = [q0,q1,q2,q3].

[0103] At the same time, the system of equations can be simplified to a quadratic function of the square of the quaternion:

[0104] A0 4 +B i q 2 +C i =0

[0105] Where: q refers to any element in the quaternion; A0, B i ,C i is the corresponding parameter of the quadratic function. It is calculated as:

[0106] A0=-2gM z

[0107] B 2\0 =-a y M y -M x g 20

[0108] B 3\1 =-a y M y -M x g 31

[0109]

[0110]

[0111] Where: A0 is the quadratic constant of the equation; B 2\0 is the linear constant of the equations for q0 and q2, B 3\1 is the linear constant of the equations of q3 and q1; N 2\0 is half the distance between the two roots of the equations q0 and q2, N 3\1 It is half the distance between the two roots of the equations q3 and q1.

[0112] From this we get the corresponding solution of the posture:

[0113]

[0114] At the same time, get a f , the pulling force can be calculated based on the weight of the drone:

[0115]

[0116] By obtaining the current state as an accurate analytical value, the calculation is simplified, the anti-interference performance of the model itself is enhanced, and the accuracy of the numerical value is improved. The technical effects produced by the present invention are as follows:

[0117] (1) Faster operation speed; the filtering process of the present invention is linear, and the relevant parameters can be calculated offline; the solution process uses an analytical method, the solution steps are simple, the performance overhead is small, and it has better performance than the numerical method.

[0118] (2) Better measurement stability: The present invention uses data from multiple sensors in conjunction with a multi-channel filter, which is independent of a single data source and results in better data stability. It can also effectively utilize redundant information of the same type from multiple sensors, while improving the stability and accuracy of acquired data. Furthermore, the attitude solution uses a set of equations, which places strong coupling requirements on the data, and the analytical solution of the data can achieve better accuracy and stability. The data vector generated using the data has better stability and accuracy than directly using real values.

[0119] (3) Faster response speed. The state estimation method of the present invention skips the PID tracking step of some estimation algorithms, has no iterative process, and only uses the current sampling data. The response speed is faster than other methods.

[0120] The foregoing description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Those skilled in the art will readily appreciate that various modifications and variations of the present invention are possible. Any modifications, equivalent substitutions, or improvements made within the spirit and principles of the present invention are intended to be encompassed within the scope of the claims.

Claims

1. A multi-sensor fusion quadrotor drone attitude estimation method, characterized in that: The method comprises the following steps: Step S1: Establish a state observation model of a quadrotor drone; Step S2: filtering the multi-channel sensor signals; Step S3, calculating the linear combination parameters of the multi-channel sensor signals; Step S4, calculating the sensor value of the fixed axis according to the linear combination parameter; Step S5: constructing an inverse solution equation of the state observation model and performing posture solution based on the observation results; The step S1 specifically includes the following sub-steps: Obtaining a theoretical acceleration value based on observation data of an acceleration sensor of the quadrotor drone; Obtaining a theoretical magnetic value based on an observation value of a magnetometer of the quadrotor drone; Establishing a determination model according to the acceleration theoretical value and the magnetic force theoretical value; According to the model, solve the attitude represented by quaternion and the acceleration provided by the pulling force of the quadrotor drone; The step S3 specifically includes the following sub-steps: Obtain magnetometer fusion signal data of three linearly independent channels; Obtaining a linear combination according to the fused signal data; Introducing the linear combination into the state observation model to obtain an ideal measurement value of the magnetic force; The step S4 specifically includes the following sub-steps: Reversely obtain corresponding linear combination parameters according to the linear combination; Applying the linear combination parameters to the state observation model to obtain a combined observation value at a moment; The step S5 specifically includes the following sub-steps: Solve the attitude quaternion and the acceleration provided by the pulling force according to the state observation model; The pulling force of the drone is obtained by calculating the acceleration provided by the pulling force.

2. The multi-sensor fusion quadrotor drone attitude estimation method according to claim 1, characterized in that: The step S2 specifically includes the following sub-steps: The error between the actual observed sensor data and the ideal observed data is a set of noises that obeys Gaussian distribution; Use Kalman filter to filter out noise from data; Within multiple channels, the data are serially filtered by using a multi-sensor optimal information fusion method.

Images