UAV Attitude Solution Method

Abstract

The present invention provides a method for solving the attitude of an unmanned aerial vehicle. The method for solving the attitude of the unmanned aerial vehicle includes a fuselage; Step 101: Obtain a transformation matrix from the geographic coordinate system to the vehicle coordinate system; Step 102: Obtain a normalized attitude quaternion from the geographic coordinate system to the vehicle coordinate system; Step 103: Based on the normalized attitude quaternion and the transformation matrix, obtain a normalized transformation matrix; Step 104: Based on the normalized transformation matrix and the normalized three-axis acceleration, obtain a gyroscope compensation error; Step 105: Based on the gyroscope compensation error and the normalized transformation matrix, obtain the attitude angle of the unmanned aerial vehicle. The method for solving the attitude of the unmanned aerial vehicle improves the accuracy of solving the attitude angle of the unmanned aerial vehicle and can accurately obtain the current attitude angle of the unmanned aerial vehicle.

Description

Technical Field

[0001] The present invention belongs to the field of unmanned aerial vehicles, and more specifically, relates to a method for solving the attitude of an unmanned aerial vehicle. Background Art

[0002] The attitude angles of an unmanned aerial vehicle include roll angle, pitch angle, and yaw angle, which are respectively defined according to the relative angular position relationship between the vehicle coordinate system and the geographic coordinate system: the roll angle is the angle of the vehicle around the longitudinal axis relative to the vertical plane, with the right deflection of the airframe being positive; the pitch angle is the angle between the longitudinal axis and the longitudinal horizontal axis generated by the vehicle rotating around the transverse horizontal axis, with the upward raising of the airframe being positive; the heading angle is the angle between the projection of the vehicle's longitudinal axis on the horizontal plane and the geographic north direction, with the airframe deflecting from north to east being positive. Since the calculation error of the heading angle channel is relatively large and there are many external sensor information sources, magnetic heading, GNSS, etc. information can generally be introduced for the heading angle. Therefore, this method mainly focuses on the calculation of the roll angle and the pitch angle.

[0003] The attitude angle is a core flight parameter of an unmanned aerial vehicle, and its calculation accuracy directly determines the performance of the flight control system of the unmanned aerial vehicle. As an important input quantity of the control law, attitude information is a necessary condition for realizing attitude control. Therefore, attitude calculation is a key technology in the design of the flight control system.

[0004] Common attitude calculation methods include the direction cosine method, Euler angle method, quaternion method, Kalman filter, and complementary filter, etc. Among them, the direction cosine method has a large amount of computation; the Euler angle method has singularities, resulting in the inability to achieve full-attitude operation; as the optimal recursive linear minimum variance estimation, the Kalman filter requires accurate modeling to obtain the system noise statistical characteristics and has a huge amount of computation, involving complex matrix calculations, and is not suitable for application to the embedded processors of mid- and low-end unmanned aerial vehicles; the complementary filter is mostly used for the attitude calculation of multi-rotor unmanned aerial vehicles based on MEMS sensors, but there is a problem of relatively large calculation errors when applied to carriers with high dynamics and large attitude changes; the quaternion method can achieve full-attitude operation, but for high-dynamic carriers facing low sampling rates, the low-subsample algorithm still cannot effectively solve the coning error in attitude calculation.

[0005] Therefore, it is necessary to develop an unmanned aerial vehicle attitude calculation method that can run based on medium- and low-precision sensors and low-cost embedded processors and can improve the accuracy of attitude calculation.

[0006] The information disclosed in the background art part of the present invention is only intended to deepen the understanding of the general background art of the present invention, and should not be regarded as an admission or any form of suggestion that this information constitutes the prior art known to those skilled in the art. Summary of the Invention

[0007] The object of the present invention is to provide a method for solving the attitude of an unmanned aerial vehicle, which can operate based on medium- and low-precision sensors and low-cost embedded processors and can improve the accuracy of attitude solution.

[0008] To achieve the above object, the present invention provides a method for solving the attitude of an unmanned aerial vehicle, which includes:

[0009] Step 101: Obtain the transformation matrix from the geographical coordinate system to the vehicle coordinate system;

[0010] Step 102: Obtain the normalized attitude quaternion from the geographical coordinate system to the vehicle coordinate system;

[0011] Step 103: Based on the normalized attitude quaternion and the transformation matrix, obtain the normalized transformation matrix;

[0012] Step 104: Based on the normalized transformation matrix and the normalized three-axis acceleration, obtain the gyroscope compensation error;

[0013] Step 105: Based on the gyroscope compensation error and the normalized transformation matrix, obtain the attitude angle of the unmanned aerial vehicle.

[0014] In any of the above technical solutions, the step of obtaining the transformation matrix from the geographical coordinate system to the vehicle coordinate system includes:

[0015] Based on the initial alignment attitude angle of the unmanned aerial vehicle and the GNSS positioning data, obtain the transformation matrix from the geographical coordinate system to the vehicle coordinate system.

[0016] In any of the above technical solutions, the step of obtaining the normalized attitude quaternion from the geographical coordinate system to the vehicle coordinate system includes:

[0017] Obtain the body angular rate in the vehicle coordinates;

[0018] Obtain the attitude quaternion from the geographical coordinate system to the vehicle coordinate system;

[0019] Based on the equivalent rotation vector simplified double-sample algorithm, obtain the normalized attitude quaternion.

[0020] In any of the above technical solutions, the step of obtaining the normalized attitude quaternion based on the equivalent rotation vector simplified double-sample algorithm includes:

[0021] Obtain the attitude quaternion of the unmanned aerial vehicle;

[0022] Obtain the angular rate of the vehicle coordinate system;

[0023] Obtain the approximate rotation vector and then obtain the normalized attitude quaternion.

[0024] In any of the above technical solutions, the step of obtaining the normalized transformation matrix based on the normalized attitude quaternion and the transformation matrix includes:

[0025] When the attitude calculation period is no more than 10 ms, solve based on the normalized attitude quaternion and the rotation vector simplified double-sample algorithm to obtain the normalized transformation matrix.

[0026] In any of the above technical solutions, the step of obtaining the gyroscope compensation error based on the normalized transformation matrix and the normalized triaxial accelerometer includes:

[0027] Perform a cross product operation on the normalized transformation matrix and the normalized triaxial acceleration to obtain the gyroscope compensation error.

[0028] In any of the above technical solutions, the step of obtaining the attitude angle of the UAV based on the gyroscope compensation error and the normalized transformation matrix includes:

[0029] Based on the gyroscope compensation error, the normalized transformation matrix, and the current normalized transformation matrix, judge the sign quadrant and calculate the attitude angle.

[0030] In any of the above technical solutions, the UAV attitude calculation method further includes:

[0031] Based on the gyroscope compensation error, obtain a new triaxial gyroscope measurement value, and repeat steps 104 to 105 based on the new gyroscope measurement value.

[0032] The beneficial effects of the present invention are as follows: The UAV attitude calculation method provided by the present invention significantly improves the calculation accuracy of the attitude conversion matrix by performing real-time correction on the quaternion; then, the gyroscope compensation error is obtained through the normalized attitude conversion matrix and the triaxial accelerometer, greatly eliminating the measurement noise of the gyroscope, and further improving the UAV attitude angle calculation accuracy, and can accurately obtain the current attitude angle of the UAV.

[0033] Other features and advantages of the present invention will be described in detail in the following specific implementation part. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] By describing the exemplary embodiments of the present invention in more detail in conjunction with the drawings, the above and other objects, features, and advantages of the present invention will become more obvious. Among them, in the exemplary embodiments of the present invention, the same reference numerals generally represent the same components.

[0035] Figure 1 Shows a schematic step flow structure diagram of a UAV attitude calculation method according to an embodiment of the present invention. DETAILED DESCRIPTION OF THE INVENTION

[0036] The preferred embodiments of the present invention will be described in more detail below. Although the preferred embodiments of the present invention are described below, it should be understood that the present invention can be implemented in various forms and should not be limited by the embodiments set forth herein. On the contrary, these embodiments are provided to make the present invention more thorough and complete, and to fully convey the scope of the present invention to those skilled in the art.

[0037] The present invention provides a method for solving the attitude of an unmanned aerial vehicle, and the method for solving the attitude of the unmanned aerial vehicle includes:

[0038] Step 101: Obtain the transformation matrix from the geographic coordinate system to the vehicle coordinate system;

[0039] Step 102: Obtain the normalized attitude quaternion from the geographic coordinate system to the vehicle coordinate system;

[0040] Step 103: Based on the normalized attitude quaternion and the transformation matrix, obtain the normalized transformation matrix;

[0041] Step 104: Based on the normalized transformation matrix and the normalized three-axis acceleration, obtain the gyroscope compensation error;

[0042] Step 105: Based on the gyroscope compensation error and the normalized transformation matrix, obtain the attitude angle of the unmanned aerial vehicle.

[0043] The method for solving the attitude of the unmanned aerial vehicle provided by the present invention significantly improves the calculation accuracy of the attitude conversion matrix by performing real-time correction on the quaternion; then, the gyroscope compensation error is obtained through the normalized attitude conversion matrix and the three-axis acceleration, greatly eliminating the measurement noise of the gyroscope, and further improving the calculation accuracy of the attitude angle of the unmanned aerial vehicle, and can accurately obtain the current attitude angle of the unmanned aerial vehicle.

[0044] In any of the above technical solutions, the step of obtaining the transformation matrix from the geographic coordinate system to the vehicle coordinate system includes: based on the initial alignment attitude angle of the unmanned aerial vehicle and the GNSS positioning data, obtain the transformation matrix from the geographic coordinate system to the vehicle coordinate system.

[0045] In some examples, the transformation matrix can be obtained by the following formula:

[0046]

[0047] Wherein, is the transformation matrix, is the pitch angle, is the roll angle, is the heading angle. The three attitude angles can be manually filled in during the initial alignment stage before flight. For UAVs that take off and land by taxiing, since the runway is generally horizontal, the roll and pitch angles are basically close to 0°. Even without initial alignment, the algorithm can converge to the normal value within a short time after the aircraft taxis out. For UAVs boosted by a rocket on a launch rack, since the pitch angle generally exceeds 10°, in order to reduce risks, manual initial alignment should be completed before launch.

[0048] In any of the above technical solutions, the steps to obtain the normalized attitude quaternion from the geographic coordinate system to the vehicle coordinate system include: obtaining the body angular rate in the vehicle coordinates; obtaining the attitude quaternion from the geographic coordinate system to the vehicle coordinate system; and obtaining the normalized attitude quaternion based on the equivalent rotation vector to simplify the dual-sample algorithm.

[0049] In some examples, the body angular rate can be calculated by the following formula:

[0050]

[0051] where, is the body angular rate, is the output of the gyroscope, is the transformation matrix, is the earth's angular rotation rate, is the position angular rate.

[0052] In any of the above technical solutions, the steps to obtain the normalized attitude quaternion based on the equivalent rotation vector to simplify the dual-sample algorithm include: obtaining the UAV attitude quaternion; obtaining the angular rate of the vehicle coordinate system; obtaining the approximate rotation vector and then obtaining the normalized attitude quaternion.

[0053] In some examples, the attitude quaternion from the geographic coordinate system to the vehicle coordinate system can be obtained by the following formula:

[0054]

[0055] where Q is the attitude quaternion, is the pitch angle, is the roll angle, is the heading angle

[0056] In some examples, the angular increment of the gyroscope within the sampling period can be obtained by the following formula:

[0057]

[0058] where, is the angular increment, 、 、 are the output values of the three-axis gyroscope within the sampling period.

[0059] Among them, the attitude quaternion can be obtained through the following formula

[0060]

[0061] Among them, is the attitude quaternion of the UAV.

[0062]

[0063] Among them, and and are the output values of the three-axis gyroscope within the sampling period.

[0064] is the previous sampling moment.

[0065] The steps to obtain the normalized attitude quaternion may include obtaining the sampling value at the current moment and the value at the previous sampling moment of the intermediate value as an additional sampling point, and respectively outputting the body angular rates corresponding to the three-axis angular rates of the previous sampling moment and the intermediate value, and the intermediate value and the current moment performing a cross product operation to obtain the equivalent rotation vector correction amount ; obtaining the attitude transformation matrix after quaternion normalization.

[0066] In any of the above technical solutions, the steps to obtain the normalized transformation matrix based on the normalized attitude quaternion and the transformation matrix include: when the attitude and heading calculation period is no more than 10 ms, solving based on the normalized attitude quaternion and the rotation vector simplified dual-sample algorithm to obtain the normalized transformation matrix.

[0067] In any of the above technical solutions, the steps to obtain the gyroscope compensation error based on the normalized transformation matrix and the normalized three-axis accelerometer include: performing a cross product operation based on the normalized transformation matrix and the normalized three-axis acceleration to obtain the gyroscope compensation error.

[0068] In some examples, the specific steps to obtain the gyroscope compensation error by performing a cross product based on the normalized transformation matrix and the normalized three-axis acceleration include:

[0069] Normalize the three-axis acceleration value;

[0070] Obtain the gravity component information of the attitude transformation matrix as follows:

[0071]

[0072] In the formula, is the gravity component of the attitude transformation matrix, is the attitude quaternion.

[0073] Perform a cross product operation on the acceleration and the gravity component to obtain the compensation value.

[0074]

[0075] In the formula, is the compensation amount calculated from the acceleration, is the output value of the three-axis acceleration, the gravity component of the attitude transformation matrix.

[0076] Based on the current attitude transformation matrix, judge the sign quadrant to calculate the attitude angle; substitute the compensation error into the measured values of the three-axis gyroscopes in the next calculation cycle and perform iterative calculations.

[0077] In any of the above technical solutions, the steps of obtaining the attitude angle of the UAV based on the gyroscope compensation error and the normalization transformation matrix include: judging the sign quadrant based on the gyroscope compensation error, the normalization transformation matrix, and the current normalization transformation matrix to calculate the attitude angle.

[0078] As a preferred solution, take the sampling value at the current moment and the value at the previous sampling moment the intermediate value as an additional sampling point, and perform cross product operations on the body angular rates corresponding to the three-axis angular rates output at the previous sampling moment and the intermediate value, and the intermediate value and the current moment respectively to obtain the equivalent rotation vector correction amount ;

[0079]

[0080] is the body angular rate corresponding to the angular rate output at the previous sampling moment, is the body angular rate corresponding to the angular rate output at the current sampling moment.

[0081] is the body angular rate corresponding to the angular rate output at the current sampling moment, is the body angular rate corresponding to the angular rate output at the previous sampling moment.

[0082]

[0083] is the rotation vector correction amount in the first half cycle of the sampling interval.

[0084]

[0085] is the rotation vector correction amount in the second half cycle of the sampling interval.

[0086]

[0087] is the attitude quaternion, is the equivalent rotation vector.

[0088] Update the change quaternion and normalize it to obtain the attitude transformation matrix;

[0089]

[0090] where, is the transformation matrix, is the attitude quaternion.

[0091] Normalize the triaxial accelerometer values;

[0092]

[0093]

[0094]

[0095] where, are the output values of the accelerometer in three directions.

[0096] Obtain the gyroscope compensation error through the cross product operation based on the attitude transformation matrix and the normalized triaxial accelerations;

[0097]

[0098] is the gravity component of the attitude transformation matrix, is the attitude quaternion.

[0099]

[0100] where, is the compensation amount calculated from the acceleration, is the output value of the triaxial accelerometer, is the gravity component of the attitude transformation matrix.

[0101] In some examples, the steps to calculate the attitude angles based on the current attitude transformation matrix by judging the sign quadrant include:

[0102] 1) Roll angle :

[0103] If < 0 and > 0, = -180° + ;

[0104] If <0 and <0, = 180° + .

[0105] 2) Pitch angle :

[0106] Is equal to the calculated value.

[0107] 3) Yaw angle :

[0108] If <0, = 180° + ;

[0109] If > 0 and <0, = 360° + ;

[0110] If > 0 and > 0, Is equal to the calculated value;

[0111] Substitute the compensation error into the measured values of the three - axis gyroscope in the next calculation cycle and perform iterative calculations.

[0112] Attitude quaternion matrix, is the roll angle, is the pitch angle, is the yaw angle.

[0113] In any of the above - mentioned technical solutions, the UAV attitude calculation method further includes: based on the gyroscope compensation error, calculate the measured values of the three - axis gyroscope, obtain the new normalized three - axis acceleration, and repeat steps 104 to 105 based on the new normalized three - axis acceleration.

[0114] The embodiments of the present invention have been described above. The above description is exemplary and not exhaustive, and is not limited to the disclosed embodiments. Many modifications and variations are obvious to those of ordinary skill in the art without departing from the scope and spirit of the described embodiments.

Claims

1. A method for calculating the attitude of an unmanned aerial vehicle, characterized in that, it includes: Step 101: Obtain the transformation matrix from the geographical coordinate system to the vehicle coordinate system; Step 102: Obtain the normalized attitude quaternion from the geographical coordinate system to the vehicle coordinate system; Step 103: Based on the normalized attitude quaternion and the transformation matrix, obtain the normalized transformation matrix; Step 104: Based on the normalized transformation matrix and the normalized three-axis acceleration, obtain the gyroscope compensation error; Step 105: Based on the gyroscope compensation error and the normalized transformation matrix, obtain the attitude angle of the unmanned aerial vehicle; The step of obtaining the normalized attitude quaternion from the geographical coordinate system to the vehicle coordinate system includes: Obtain the body angular velocity in the vehicle coordinates; Obtain the attitude quaternion from the geographical coordinate system to the vehicle coordinate system; Based on the equivalent rotation vector simplified dual-sample algorithm, obtain the normalized attitude quaternion.

2. The method for calculating the attitude of an unmanned aerial vehicle according to claim 1, characterized in that, the step of obtaining the transformation matrix from the geographical coordinate system to the vehicle coordinate system includes: Based on the initial alignment attitude angle of the unmanned aerial vehicle and the GNSS positioning data, obtain the transformation matrix from the geographical coordinate system to the vehicle coordinate system.

3. The method for calculating the attitude of an unmanned aerial vehicle according to claim 2, characterized in that, the step of obtaining the normalized attitude quaternion based on the equivalent rotation vector simplified dual-sample algorithm includes: Obtain the attitude quaternion of the unmanned aerial vehicle; Obtain the angular velocity of the vehicle coordinate system; Obtain the approximate rotation vector and then obtain the normalized attitude quaternion.

4. The method for calculating the attitude of an unmanned aerial vehicle according to claim 3, characterized in that, the step of obtaining the normalized transformation matrix based on the normalized attitude quaternion and the transformation matrix includes: When the attitude calculation period is not greater than 10 ms, solve based on the normalized attitude quaternion and the rotation vector simplified dual-sample algorithm to obtain the normalized transformation matrix.

5. The method for calculating the attitude of an unmanned aerial vehicle according to claim 1, characterized in that, the step of obtaining the gyroscope compensation error based on the normalized transformation matrix and the normalized three-axis acceleration includes: Perform a cross product operation on the normalized transformation matrix and the normalized three-axis acceleration to obtain the gyroscope compensation error.

6. The method for calculating the attitude of an unmanned aerial vehicle according to claim 1, characterized in that, the step of obtaining the attitude angle of the unmanned aerial vehicle based on the gyroscope compensation error and the normalized transformation matrix includes: Based on the gyroscope compensation error, the normalized transformation matrix and the current normalized transformation matrix, judge the sign quadrant and solve the attitude angle.

7. The method for calculating the attitude of an unmanned aerial vehicle according to any one of claims 1 to 6, characterized in that, it further includes: Based on the gyroscope compensation error, obtain the new three-axis gyroscope measurement value, and repeat steps 104 to 105 based on the new gyroscope measurement value.

Images