CN103363992B

CN103363992B - Based on four rotor wing unmanned aerial vehicle attitude heading reference system calculation methods of Gradient Descent

Info

Publication number: CN103363992B
Application number: CN201310272266.7A
Authority: CN
Inventors: 鲜斌; 宋英麟; 茹滨超
Original assignee: Tianjin University
Current assignee: Tianjin University
Priority date: 2013-06-29
Filing date: 2013-06-29
Publication date: 2015-12-09
Anticipated expiration: 2033-06-29
Also published as: CN103363992A

Abstract

A kind of four rotor wing unmanned aerial vehicle attitude heading reference system calculation methods based on Gradient Descent, comprise: gather miniature four rotor wing unmanned aerial vehicle attitude informations by gyroscope, acceleration transducer, geomagnetic sensor, wherein, the angular speed of miniature four rotor wing unmanned aerial vehicles of gyroscope survey, the linear acceleration of miniature four rotor wing unmanned aerial vehicles measured by acceleration transducer, and geomagnetic sensor measures the Magnetic Field of miniature four rotor wing unmanned aerial vehicle present positions; Apply based on hypercomplex number update algorithm degree of the will speed up sensor of Gradient Descent, geomagnetic sensor to data and the data that gather of gyroscope merge, revise gyrostatic drift; Application Quaternion Method resolves miniature four rotor wing unmanned aerial vehicle attitude angle information.The present invention adopts the data fusion method based on gradient descent method, make full use of acceleration transducer and geomagnetic sensor data, the drift of gyro data is revised in real time, export 3 d pose angle information, thus minimizing systematic error, effectively improve attitude measurement accuracy and stability.

Description

Four-rotor unmanned aerial vehicle attitude and heading reference system resolving method based on gradient descent

Technical Field

The invention relates to attitude and heading data measurement of a quad-rotor unmanned aerial vehicle. In particular to a method for calculating a four-rotor unmanned aerial vehicle attitude and heading reference system based on gradient descent.

Background

The attitude and heading reference system is a core device in an airborne electronic system of the quad-rotor unmanned aerial vehicle, and is responsible for providing real-time and reliable flight state measurement data for a flight control system of the unmanned aerial vehicle. At present, the size of a common four-rotor unmanned aerial vehicle body is small, the load is light, and the weight and size limitation on an inertial navigation unit is strict, so that a micro attitude navigation system based on an MEMS (micro electro mechanical system) technology is mostly adopted to provide flight state measurement data.

At present, the composition scheme and the application of various course attitude measurement systems exist at home and abroad. A Cristal100 course attitude reference system of French general mechanical electrical appliances adopts a ring laser gyroscope and a quartz accelerometer, and a Kalman filter is adopted in the system, so that the system can be conveniently calibrated on the ground, on a ship and in flight. The Cristal100 heading and attitude reference system may be used for fixed wing or rotary wing aircraft. The NAV440 attitude reference system of Crossbow, USA, utilizes MEMS inertial device and GPS technology comprehensively, and embodies the latest measurement and control technology based on MEMS. The method is widely applied to the fields of aviation, land and ocean navigation, tracking control, platform stabilization, ROV/AGV control, UAV/RPV control, accurate cultivation and the like. (Cross bowtechnology. NAVS440GPS-AID MID SinertialNavigation System [ EB/OL ]. http:// www.xbow.com.cn /)

In China, the development of related research work of the miniature attitude heading measurement system is late. The core strength of the development of the research work of the miniature attitude and heading measurement system is mainly concentrated in some scientific research institutions and colleges and universities. The miniature iFLY-G2 integrated navigation system developed by Beijing aerospace university is a small six-degree-of-freedom integrated navigation system. The system uses an MEMS gyroscope and an acceleration sensor as core devices, and the system can solve course attitude information by depending on an internal DSP processor and can provide real-time information such as Euler angle, three-dimensional angular rate and the like for an onboard controller. The iFLY-G2 provides accurate and comprehensive measurement information for the stability and control of various aircraft. The iFLY-G2 provides two combined navigation modes of AHRS/DR and GPS/INS, when the GPS information is effective, the combined navigation mode is automatically switched to the GPS/INS, and information of 50HZ position, speed, attitude and the like is provided; when the GPS information is invalid, the mode is automatically switched to an AHRS/DR mode (the AHRS mode containing dead reckoning), and the safety of flight is ensured.

In recent years, with the development of MEMS technology, micro attitude heading measurement systems have appeared. The system generally comprises a sensing device based on MEMS technology, an application specific integrated circuit and an embedded microcontroller, and can provide real-time navigation information such as gestures and the like for a motion carrier. The micro attitude navigation system has the characteristics of small volume, low cost, autonomy, real time, strong anti-jamming capability and the like (in Junjie. micro inertial combination system design and research [ D ] Harbin engineering university, 2007.) (Liu. MEMS-based low-cost MIMU application research [ D ] national defense science and technology university, 2004.), but the precision mainly depends on the precision of an inertial device, the measurement precision of the inertial device is hardly greatly improved from the aspects of hardware structure design and process alone, the sensor drift is large, and system errors are accumulated along with time, so that the micro attitude navigation system is not suitable for application of long-time carrier attitude determination.

The attitude and heading reference system consists of a plurality of axial sensors and can provide heading, rolling and pitching information for the aircraft, and the system can provide accurate and reliable attitude and navigation information for the aircraft. In order to improve the stability of the unmanned aerial vehicle in the flight process, the attitude heading reference system needs to provide the current attitude angle of the unmanned aerial vehicle in real time for an onboard controller to use. However, low cost sensor drift is typically large and system errors accumulate over time, which is not suitable for use with long duration carrier attitude determinations, which limits the application of course attitude measurement systems to some extent.

The technical reports of the existing documents are analyzed, and the problems that the sensor has larger drift and low precision are the main problems of the existing miniature four-rotor unmanned aerial vehicle attitude and heading reference system.

The miniature attitude navigation system has the characteristics of small volume, low cost, autonomy, real time, strong interference resistance and the like, but the precision of the miniature attitude navigation system mainly depends on the precision of an inertial sensor device, the measurement precision of the inertial sensor device is difficult to be greatly improved from the aspects of hardware structure design and process, the data drift of the sensor is large, and system errors are accumulated along with time, so that the miniature attitude navigation system is not suitable for the application of long-time carrier attitude determination.

With the development of the strapdown inertial navigation technology, in order to describe the attitude information of the carrier more simply and conveniently, a quaternion is adopted as a mathematical tool, and the quaternion is used for making up the defects of 3 Euler angle parameters which generally describe the angular motion of a rigid body in designing a control system. Quaternions may describe the rotation of a coordinate system or a vector with respect to a coordinate system, with the scalar portion of the quaternion representing the cosine of the 1/2 corner and the vector portion representing the direction of the instantaneous axis of rotation and the direction cosine between the instantaneous axis of rotation and the axis of the reference coordinate system. Therefore, a quaternion indicates both the direction of the rotation axis and the magnitude of the rotation angle, and is often referred to as a rotation quaternion. Research on data acquisition and processing technology of a strapdown inertial navigation system based on MIMU [ D ]. Harbin: Harbin engineering university, 2010) acquires angular rate data of a carrier through a gyroscope, and the data is resolved by using a quaternion updating method to obtain attitude information of the carrier.

Disclosure of Invention

The invention aims to solve the technical problem of providing a gradient descent-based attitude and heading reference system calculation method for a quadrotor unmanned aerial vehicle, which can obtain an attitude angle measurement value with higher precision, realize the attitude information estimation of the quadrotor unmanned aerial vehicle and improve the attitude and position control precision of the quadrotor unmanned aerial vehicle.

The technical scheme adopted by the invention is as follows: a method for calculating a four-rotor unmanned aerial vehicle attitude and heading reference system based on gradient descent comprises the following steps:

1) acquiring attitude information of the miniature quad-rotor unmanned aerial vehicle through a gyroscope, an acceleration sensor and a geomagnetic sensor, wherein the gyroscope measures the angular rate of the miniature quad-rotor unmanned aerial vehicle, the acceleration sensor measures the linear acceleration of the miniature quad-rotor unmanned aerial vehicle, and the geomagnetic sensor measures the magnetic field information of the position of the miniature quad-rotor unmanned aerial vehicle;

2) fusing data acquired by the acceleration sensor and the geomagnetic sensor with data acquired by the gyroscope by using a gradient descent-based quaternion updating algorithm, and correcting the drift of the gyroscope;

3) and resolving the attitude angle information of the miniature quad-rotor unmanned aerial vehicle by using a quaternion method.

The drift of the correction gyroscope in the step 2) is as follows:

the measured value of the sensor under the rigid body coordinate system is set as

d = [\begin{matrix} 0 & d_{x} & d_{y} & d_{z} \end{matrix}]

The actual value set in the geodetic coordinate system is

s = [\begin{matrix} 0 & s_{x} & s_{y} & s_{z} \end{matrix}]

To minimize the error, the minimum of the solution function f (Q, d, s) is required: minf (Q, d, s),

<math> <mrow> <mi>f</mi> <mrow> <mo>(</mo> <mi>Q</mi> <mo>,</mo> <mi>d</mi> <mo>,</mo> <mi>s</mi> <mo>)</mo> </mrow> <mo>=</mo> <msup> <mi>Q</mi> <mo>*</mo> </msup> <mo>&CircleTimes;</mo> <mi>d</mi> <mo>&CircleTimes;</mo> <mi>Q</mi> <mo>-</mo> <mi>s</mi> </mrow> </math>

wherein,

Q = [\begin{matrix} q_{1} & q_{2} & q_{3} & q_{4} \end{matrix}]

is a rotation quaternion, Q, of a rigid body coordinate system to a geodetic coordinate system^*Is a conjugate quaternion, and is,

will be provided with

d = [\begin{matrix} 0 & d_{x} & d_{y} & d_{z} \end{matrix}],

、

s = [\begin{matrix} 0 & s_{x} & s_{y} & s_{z} \end{matrix}],

、

Q = [\begin{matrix} q_{1} & q_{2} & q_{3} & q_{4} \end{matrix}]

Substituted type

Obtaining:

f (Q, d, s) = [\begin{matrix} {2 d}_{x} (\frac{1}{2} - q_{3}^{2} - q_{4}^{2}) + {2 d}_{y} (q_{1} q_{4} + q_{2} q_{3}) + {2 d}_{z} (q_{2} q_{4} - q_{1} q_{3}) - s_{x} \\ {2 d}_{x} (q_{2} q_{3} - q_{1} q_{4}) + {2 d}_{y} (\frac{1}{2} - q_{2}^{2} - q_{4}^{2}) + {2 d}_{z} (q_{1} q_{2} + q_{3} q_{4}) - s_{y} \\ {2 d}_{x} (q_{1} q_{3} + q_{2} q_{4}) + {2 d}_{z} (\frac{1}{2} - q_{2}^{2} - q_{3}^{2}) + {2 d}_{y} (q_{3} q_{4} - q_{1} q_{2}) - s_{z} \end{matrix}]

according to the gradient descent algorithm, the unit vector of the negative gradient search of the f (Q, d, s) function is:

wherein J (Q, d) is the Jacobian matrix of f (Q, d, s), J^T(Q, d) is the transpose of the Jacobian matrix of f (Q, d, s);

J (Q, d) = [\begin{matrix} {2 d}_{y} q_{4} - {2 d}_{z} q_{3} & {2 d}_{y} q_{3} + {2 d}_{z} q_{4} \\ - {2 d}_{x} q_{4} + {2 d}_{z} q_{2} & {2 d}_{x} q_{3} - {4 d}_{y} q_{2} + {2 d}_{z} q_{1} \\ {2 d}_{x} q_{3} - {2 d}_{y} q_{2} & {2 d}_{x} q_{4} - {2 d}_{y} q_{1} - {4 d}_{z} q_{2} \end{matrix}

\begin{matrix} - {4 d}_{x} q_{3} + {2 d}_{y} q_{2} - {2 d}_{z} q_{1} & - {4 d}_{x} q_{4} + {2 d}_{y} q_{1} + {2 d}_{z} q_{2} \\ {2 d}_{x} q_{2} + {2 d}_{z} q_{4} & - d_{x} q_{1} - {4 d}_{y} q_{4} + {2 d}_{z} q_{3} \\ {2 d}_{x} q_{1} + {2 d}_{y} q_{4} - {4 d}_{z} q_{3} & {2 d}_{x} q_{2} + {2 d}_{y} q_{3} \end{matrix}]

through the derivation, the quaternion update equation with the gradient descent compensation algorithm is obtained as follows:

<math> <mrow> <msub> <mi>Q</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>Q</mi> <mi>k</mi> </msub> <mo>+</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msub> <mi>Q</mi> <mi>k</mi> </msub> <mo>&CircleTimes;</mo> <mi>ω</mi> <mo>-</mo> <mi>μ</mi> <mfrac> <mrow> <mo>&dtri;</mo> <mi>f</mi> <mrow> <mo>(</mo> <msub> <mi>Q</mi> <mi>k</mi> </msub> <mo>,</mo> <mi>d</mi> <mo>,</mo> <mi>s</mi> <mo>)</mo> </mrow> </mrow> <mrow> <mo>|</mo> <mo>|</mo> <mo>&dtri;</mo> <mi>f</mi> <mrow> <mo>(</mo> <msub> <mi>Q</mi> <mi>k</mi> </msub> <mo>,</mo> <mi>d</mi> <mo>,</mo> <mi>s</mi> <mo>)</mo> </mrow> <mo>|</mo> <mo>|</mo> </mrow> </mfrac> </mrow> </math>

wherein Q_k+1Is a quaternion after the drift correction of the gyroscope at the current moment, mu is a step length, k is an integer which is more than or equal to zero, omega is the angular velocity measured by the gyroscope,for multiplication of quaternions, Q_kAnd the quaternion is obtained after the drift of the gyroscope is corrected at the previous moment.

Step 3) the resolving of the attitude angle information of the miniature quad-rotor unmanned aerial vehicle is as follows:

setting the yaw angle of the micro quad-rotor unmanned aerial vehicle as psi, the pitch angle as theta, the roll angle as gamma, taking a geodetic coordinate system as a navigation coordinate system, and defining x_g、y_g、z_gThe rigid body coordinate system can be converted into a navigation coordinate system through three times of basic rotation coordinate transformation when the rigid body coordinate system points to east, north and sky respectively, and the attitude angle information of the carrier can be calculated through the following formula

<math> <mrow> <mfenced open='{' close=''> <mtable> <mtr> <mtd> <mi>θ</mi> <mo>=</mo> <mi>arcsin</mi> <mo>[</mo> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>3</mn> </msub> <msub> <mi>q</mi> <mn>4</mn> </msub> <mo>+</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>)</mo> </mrow> <mo>]</mo> </mtd> </mtr> <mtr> <mtd> <mi>γ</mi> <mo>=</mo> <mi>arctan</mi> <mo>[</mo> <mo>-</mo> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>2</mn> </msub> <msub> <mi>q</mi> <mn>4</mn> </msub> <mo>-</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>)</mo> </mrow> <mo>/</mo> <msubsup> <mi>q</mi> <mn>4</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>q</mi> <mn>3</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>q</mi> <mn>2</mn> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>q</mi> <mn>1</mn> <mn>2</mn> </msubsup> <mo>]</mo> </mtd> </mtr> <mtr> <mtd> <mi>ψ</mi> <mo>=</mo> <mi>arctan</mi> <mo>[</mo> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>2</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>4</mn> </msub> <mo>)</mo> </mrow> <mo>/</mo> <msubsup> <mi>q</mi> <mn>3</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>q</mi> <mn>4</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>q</mi> <mn>1</mn> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>q</mi> <mn>2</mn> <mn>2</mn> </msubsup> <mo>]</mo> </mtd> </mtr> </mtable> </mfenced> <mo>.</mo> </mrow> </math>

According to the method for resolving the data of the attitude and heading reference system of the micro quad-rotor unmanned aerial vehicle based on gradient descent, a data fusion method based on a gradient descent method is adopted, the data of the acceleration sensor and the geomagnetic sensor are fully utilized, the drift of gyroscope data is corrected in real time, and three-dimensional attitude angle information is output, so that the system error is reduced, and the attitude measurement precision and stability are effectively improved. The invention has the following characteristics:

1. the calculation amount of the calculation process is small. The traditional attitude heading reference system data resolving method based on the Kalman filter generally needs a large amount of matrix operation, has high requirement on the operation speed of a processor in practical application, and improves the cost of the attitude heading reference system. The resolving method provided by the invention is small in calculation amount, low in requirement on the operation speed of the processor and suitable for a low-cost attitude heading reference system of the micro unmanned aerial vehicle.

2. The precision and the stability are high. The data resolving method provided by the invention integrates data information of the acceleration sensor and the geomagnetic sensor, overcomes the problem of larger data drift of the gyroscope, compensates the drift of the gyroscope by using data of the accelerometer and the magnetometer, corrects a system error generated by the drift of the gyroscope, and improves the measurement precision of the attitude and heading reference system.

3. The invention adopts a hardware architecture which takes a low-cost MEMS sensor as a basis and takes a low-cost ARM data processor as a core. Therefore, compared with the existing attitude heading reference system, the invention has the advantages of small volume, light weight, low power consumption and the like, and greatly reduces the cost of the miniature attitude navigation system.

Drawings

FIG. 1 is a flow chart of a gradient descent based quaternion compensation algorithm;

FIG. 2a is the result of a roll angle experiment prior to data processing;

FIG. 2b shows experimental results of roll angle after data processing;

FIG. 3a is the results of a pitch angle experiment prior to data processing;

FIG. 3b is the results of a pitch angle experiment after data processing;

FIG. 4a is the results of a yaw angle experiment prior to data processing;

FIG. 4b shows the experimental results of the yaw angle after data processing.

Detailed Description

The method for solving the attitude and heading reference system of the quad-rotor unmanned aerial vehicle based on gradient descent is described in detail below by combining the embodiment and the attached drawings.

As shown in fig. 1, the method for calculating the attitude and heading reference system of the quad-rotor unmanned aerial vehicle based on gradient descent comprises the following steps:

the drift of the correction gyroscope is as follows:

d = [\begin{matrix} 0 & d_{x} & d_{y} & d_{z} \end{matrix}]

The actual value set in the geodetic coordinate system is

s = [\begin{matrix} 0 & s_{x} & s_{y} & s_{z} \end{matrix}]

<math> <mrow> <mi>f</mi> <mrow> <mo>(</mo> <mi>Q</mi> <mo>,</mo> <mi>d</mi> <mo>,</mo> <mi>s</mi> <mo>)</mo> </mrow> <mo>=</mo> <msup> <mi>Q</mi> <mo>*</mo> </msup> <mo>&CircleTimes;</mo> <mi>d</mi> <mo>&CircleTimes;</mo> <mi>Q</mi> <mo>-</mo> <mi>s</mi> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo> </mrow> </mrow> </math>

wherein,

Q = [\begin{matrix} q_{1} & q_{2} & q_{3} & q_{4} \end{matrix}]

will be provided with

d = [\begin{matrix} 0 & d_{x} & d_{y} & d_{z} \end{matrix}],

s = [\begin{matrix} 0 & s_{x} & s_{y} & s_{z} \end{matrix}],

Q = [\begin{matrix} q_{1} & q_{2} & q_{3} & q_{4} \end{matrix}]

Substituting the formula (1) to obtain:

f (Q, d, s) = [\begin{matrix} {2 d}_{x} (\frac{1}{2} - q_{3}^{2} - q_{4}^{2}) + {2 d}_{y} (q_{1} q_{4} + q_{2} q_{3}) + {2 d}_{z} (q_{2} q_{4} - q_{1} q_{3}) - s_{x} \\ {2 d}_{x} (q_{2} q_{3} - q_{1} q_{4}) + {2 d}_{y} (\frac{1}{2} - q_{2}^{2} - q_{4}^{2}) + {2 d}_{z} (q_{1} q_{2} + q_{3} q_{4}) - s_{y} \\ {2 d}_{x} (q_{1} q_{3} + q_{2} q_{4}) + {2 d}_{z} (\frac{1}{2} - q_{2}^{2} - q_{3}^{2}) + {2 d}_{y} (q_{3} q_{4} - q_{1} q_{2}) - s_{z} \end{matrix}] - - - (2)

J (Q, d) = [\begin{matrix} {2 d}_{y} q_{4} - {2 d}_{z} q_{3} & {2 d}_{y} q_{3} + {2 d}_{z} q_{4} \\ - {2 d}_{x} q_{4} + {2 d}_{z} q_{2} & {2 d}_{x} q_{3} - {4 d}_{y} q_{2} + {2 d}_{z} q_{1} \\ {2 d}_{x} q_{3} - {2 d}_{y} q_{2} & {2 d}_{x} q_{4} - {2 d}_{y} q_{1} - {4 d}_{z} q_{2} \end{matrix}

(5)

\begin{matrix} - {4 d}_{x} q_{3} + {2 d}_{y} q_{2} - {2 d}_{z} q_{1} & - {4 d}_{x} q_{4} + {2 d}_{y} q_{1} + {2 d}_{z} q_{2} \\ {2 d}_{x} q_{2} + {2 d}_{z} q_{4} & - d_{x} q_{1} - {4 d}_{y} q_{4} + {2 d}_{z} q_{3} \\ {2 d}_{x} q_{1} + {2 d}_{y} q_{4} - {4 d}_{z} q_{3} & {2 d}_{x} q_{2} + {2 d}_{y} q_{3} \end{matrix}]

<math> <mrow> <msub> <mi>Q</mi> <mrow> <mi>k</mi> <mo>+</mo> <mn>1</mn> </mrow> </msub> <mo>=</mo> <msub> <mi>Q</mi> <mi>k</mi> </msub> <mo>+</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msub> <mi>Q</mi> <mi>k</mi> </msub> <mo>&CircleTimes;</mo> <mi>ω</mi> <mo>-</mo> <mi>μ</mi> <mfrac> <mrow> <mo>&dtri;</mo> <mi>f</mi> <mrow> <mo>(</mo> <msub> <mi>Q</mi> <mi>k</mi> </msub> <mo>,</mo> <mi>d</mi> <mo>,</mo> <mi>s</mi> <mo>)</mo> </mrow> </mrow> <mrow> <mo>|</mo> <mo>|</mo> <mo>&dtri;</mo> <mi>f</mi> <mrow> <mo>(</mo> <msub> <mi>Q</mi> <mi>k</mi> </msub> <mo>,</mo> <mi>d</mi> <mo>,</mo> <mi>s</mi> <mo>)</mo> </mrow> <mo>|</mo> <mo>|</mo> </mrow> </mfrac> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>6</mn> <mo>)</mo> </mrow> </mrow> </math>

Resolving the attitude angle information of the miniature quad-rotor unmanned aerial vehicle is as follows:

<math> <mrow> <mfenced open='{' close=''> <mtable> <mtr> <mtd> <mi>θ</mi> <mo>=</mo> <mi>arcsin</mi> <mo>[</mo> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>3</mn> </msub> <msub> <mi>q</mi> <mn>4</mn> </msub> <mo>+</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>)</mo> </mrow> <mo>]</mo> </mtd> </mtr> <mtr> <mtd> <mi>γ</mi> <mo>=</mo> <mi>arctan</mi> <mo>[</mo> <mo>-</mo> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>2</mn> </msub> <msub> <mi>q</mi> <mn>4</mn> </msub> <mo>-</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>)</mo> </mrow> <mo>/</mo> <msubsup> <mi>q</mi> <mn>4</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>q</mi> <mn>3</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>q</mi> <mn>2</mn> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>q</mi> <mn>1</mn> <mn>2</mn> </msubsup> <mo>]</mo> </mtd> </mtr> <mtr> <mtd> <mi>ψ</mi> <mo>=</mo> <mi>arctan</mi> <mo>[</mo> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>2</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>4</mn> </msub> <mo>)</mo> </mrow> <mo>/</mo> <msubsup> <mi>q</mi> <mn>3</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>q</mi> <mn>4</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>q</mi> <mn>1</mn> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>q</mi> <mn>2</mn> <mn>2</mn> </msubsup> <mo>]</mo> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>7</mn> <mo>)</mo> </mrow> <mo>.</mo> </mrow> </math>

In conclusion, the method for solving the attitude and heading reference system of the four-rotor unmanned aerial vehicle based on gradient descent corrects the problem of large drift of a low-cost sensor by utilizing multi-sensor data fusion, and improves the measurement accuracy of the miniature attitude and heading system.

The comprehensive performance of the gradient descent-based four-rotor unmanned aerial vehicle attitude and heading reference system calculation method is verified and explained through experimental results. FIG. 2a is the result of a roll angle experiment prior to data processing; FIG. 2b shows experimental results of roll angle after data processing; FIG. 3a is the results of a pitch angle experiment prior to data processing; FIG. 3b is the results of a post-processing pitch angle experiment with time (in milliseconds) on the abscissa and pitch/roll angle (in degrees) on the ordinate, and a pre-processing angle drift of about 10 degrees/min. The data are resolved by the data resolving method of the gradient descent-based attitude and heading reference system of the miniature four-rotor unmanned aerial vehicle, and after the data are processed, the angle drift is about 0.3 degree/minute. FIG. 4a is the results of a yaw angle experiment prior to data processing; FIG. 4b shows the experimental results of the yaw angle after data processing, the data are calculated by the method for calculating the attitude and heading reference system of the quad-rotor unmanned aerial vehicle based on gradient descent, and after the data are processed, the angle drift is about 2 degrees/min. The result analysis shows that the four-rotor unmanned aerial vehicle attitude and heading reference system calculation method based on gradient descent can effectively reduce the angle drift, improve the stability and accuracy of the miniature attitude and heading system, and can be effectively applied to attitude and position control of the miniature four-rotor unmanned aerial vehicle system.

Claims

1. A method for calculating a four-rotor unmanned aerial vehicle attitude and heading reference system based on gradient descent is characterized by comprising the following steps:

2) fusing data acquired by an acceleration sensor and a geomagnetic sensor with data acquired by a gyroscope by applying a gradient descent-based quaternion updating algorithm, and correcting the drift of the gyroscope, wherein the correction of the drift of the gyroscope is as follows:

let d be [0d ] as the measured value of the gyroscope in the rigid coordinate system_xd_yd_z]The actual value given in the geodetic coordinate system is s ═ 0s_xs_ys_z]To minimize the error, the minimum of the solution function f (Q, d, s) is required: minf (Q, d, s),

wherein Q ═ Q₁q₂q₃q₄]Is a rotation quaternion, Q, of a rigid body coordinate system to a geodetic coordinate system^*Is a conjugate quaternion, and is,

d is ═ 0d_xd_yd_z]、s＝[0s_xs_ys_z]、Q＝[q₁q₂q₃q₄]Substituted type

Obtaining:

f (Q, d, s) = [\begin{matrix} 2 d_{x} (\frac{1}{2} - q_{3}^{2} - q_{4}^{2}) + 2 d_{y} (q_{1} q_{4} + q_{2} q_{3}) + 2 d_{z} (q_{2} q_{4} - q_{1} q_{3}) - s_{x} \\ 2 d_{x} (q_{2} q_{3} - q_{1} q_{4}) + 2 d_{y} (\frac{1}{2} - q_{2}^{2} - q_{4}^{2}) + 2 d_{z} (q_{1} q_{2} + q_{3} q_{4}) - s_{y} \\ 2 d_{x} (q_{1} q_{3} + q_{2} q_{4}) + 2 d_{z} (\frac{1}{2} - q_{2}^{2} - q_{3}^{2}) + 2 d_{y} (q_{3} q_{4} - q_{1} q_{2}) - s_{z} \end{matrix}]

according to the gradient descent algorithm, the unit vector of the nf (Q, d, s) function negative gradient search is:

wherein Q_k+1Is a quaternion after the drift correction of the gyroscope at the current moment, mu is a step length, k is an integer which is more than or equal to zero, omega is the angular velocity measured by the gyroscope,for multiplication of quaternions, Q_kThe quaternion is obtained after the drift of the gyroscope is corrected at the previous moment;

2. The gradient descent-based quad-rotor unmanned aerial vehicle attitude and heading reference system calculation method according to claim 1, wherein the calculation of the attitude angle information of the miniature quad-rotor unmanned aerial vehicle in the step 3) is as follows:

<math> <mrow> <mfenced open = "{" close = ""> <mtable> <mtr> <mtd> <mrow> <mi>θ</mi> <mo>=</mo> <mi>arctan</mi> <mo>[</mo> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>3</mn> </msub> <msub> <mi>q</mi> <mn>4</mn> </msub> <mo>+</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>2</mn> </msub> <mo>)</mo> </mrow> <mo>]</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>γ</mi> <mo>=</mo> <mi>arctan</mi> <mo>[</mo> <mo>-</mo> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>2</mn> </msub> <msub> <mi>q</mi> <mn>4</mn> </msub> <mo>-</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>)</mo> </mrow> <mo>/</mo> <msubsup> <mi>q</mi> <mn>4</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>q</mi> <mn>3</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>q</mi> <mn>2</mn> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>q</mi> <mn>1</mn> <mn>2</mn> </msubsup> <mo>]</mo> </mrow> </mtd> </mtr> <mtr> <mtd> <mrow> <mi>ψ</mi> <mo>=</mo> <mi>arctan</mi> <mo>[</mo> <mn>2</mn> <mrow> <mo>(</mo> <msub> <mi>q</mi> <mn>2</mn> </msub> <msub> <mi>q</mi> <mn>3</mn> </msub> <mo>-</mo> <msub> <mi>q</mi> <mn>1</mn> </msub> <msub> <mi>q</mi> <mn>4</mn> </msub> <mo>)</mo> </mrow> <mo>/</mo> <msubsup> <mi>q</mi> <mn>3</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>q</mi> <mn>4</mn> <mn>2</mn> </msubsup> <mo>-</mo> <msubsup> <mi>q</mi> <mn>1</mn> <mn>2</mn> </msubsup> <mo>+</mo> <msubsup> <mi>q</mi> <mn>2</mn> <mn>2</mn> </msubsup> <mo>]</mo> </mrow> </mtd> </mtr> </mtable> </mfenced> <mo>.</mo> </mrow> </math>