CN102692225A - Attitude heading reference system for low-cost small unmanned aerial vehicle - Google Patents

Abstract

The invention relates to an attitude heading reference system for a low-cost small unmanned aerial vehicle. The attitude heading reference system comprises an angular rate gyro, an accelerometer, a global position system (GPS), an angular rate gyro operation module, an aiding module and a Kalman filter, wherein the angular rate gyro is used for measuring a roll angular rate, a pitch angular rate and a yaw angular rate of an aerial vehicle; the accelerometer is used for measuring a weight component of gravity in an aerial vehicle coordinate axis system; the GPS is used for measuring a track azimuth of an aerial vehicle; the angular rate gyro operation module is used for computing a pitch angle q1, a yaw angle y1 and a roll angle f1 with offsets; the aiding module is used for estimating an estimated roll angle f2 and an estimated pitch angle q2 and using the track azimuth measured by the GPS as an estimated yaw angle y2; and the Kalman filter is used for fusion of data produced by the angular rate gyro operation module and the aiding module to acquire a final attitude angle [f q y]<T>. The attitude heading reference system reduces a cost of a small unmanned aerial vehicle system and a complex degree of a heading reference system and improves the precision of attitude estimation.

Description

A kind of attitude heading reference system that is used for low-cost SUAV

Technical field

The present invention relates to a kind of attitude heading reference system, particularly a kind of attitude heading reference system that is used for low-cost SUAV based on sensor fusion techniques.

Background technology

(Attitude Heading Reference System AHRS), is called for short boat appearance system to the attitude heading reference system, is used for confirming the orientation of motion carrier in the space.The attitude information that is provided by the attitude heading reference system can be widely used in Navigation, Guidance and Control.For the unmanned plane of autonomous flight, boat appearance system is particularly important, and it provides necessary attitude feedback for the inner looping of control system.Usually, the confirming of attitude information depends on high-precision angular rate gyroscope, yet high accuracy gyroscope exists defectives such as cost height, weight are big, complex structure obviously can not be applicable to low-cost SUAV system.Therefore, under the restriction condition of low-cost sensor,, the SUAV system always is the problem of domestic and international association area broad research for providing a cover attitude heading reference system that calculated amount is little, reliability is high.

Generally, the attitude of motion carrier is that (f, q y) describe with Eulerian angle., can the measured value of angular rate gyroscope be carried out integration, thereby obtain the attitude information of carrier with the relation between the Eulerian angle rate of change according to the angular speed of carrier movement, but this method for the having relatively high expectations of Gyro Precision (output error less than 0.1 °/h).Low-cost SUAV adopts MEMS (Micro Flectrical Mechanical System) sensor usually; Though possess characteristics such as cost is low, volume is little; The precision of this type of sensor is relatively poor; Output error can produce bigger drift during the integrating gyroscope output valve between 10 °～100 °/h, thereby can not directly be used for the attitude estimation.

Attitude information can not rely on angular rate gyroscope yet, and adopts measurement vector to confirm.Adopt measurement vector to confirm that the ultimate principle of attitude is: with two measurement vectors of conllinear not under two coordinate systems respectively, two coordinate systems to be overlapped, calculate Eulerian angle again through three rotations.Some documents are studied to this no gyrosystem (Gyro-Free Systems).For example, use accelerometer, magnetometer and GPS (Global Positioning System), obtain attitude information through finding the solution the Wahba problem as sensor; Also can adopt gps carrier phase observations amount to carry out attitude and estimate through many antenna GPS.Yet under a lot of actual conditions, this no gyro boat appearance system can not use separately.At first, no gyro boat appearance system can not provide high bandwidth attitude information; Secondly, do specificly when motor-driven when carrier, the attitude information that uses this method to estimate can exist than mistake.For example when the unmanned plane banked turn, accelerometer can be responsive to except that acceleration of gravity centripetal acceleration, it is inaccurate to cause calculating attitude; At last, the attitude information that provides of no gyro boat appearance system comprises bigger noise.

For these reasons, the no gyrosystem that can use sensor fusion (Sensor Fusion) technology that the same accelerometer of angular rate gyroscope, magnetic strength are taken into account compositions such as GPS merges, and they are complemented one another, and learns from other's strong points to offset one's weaknesses.Because angular rate gyroscope can provide high bandwidth that inclined to one side attitude information is arranged, and can be used to " smoothly " no gyrosystem; Do not have gyrosystem and then be used for the drift that the correction angle rate gyro produces, therefore become corrective system (Aiding System) yet.Like this, utilize suitable wave filter that the two is combined after, just can obtain high bandwidth and not have inclined to one side attitude information solution.The core of sensor fusion techniques is the wave filter that is used to merge different sensors, and the method for using wave filter fusion angle rate gyro and no gyrosystem to survey appearance is widely used in the attitude problem identificatioin of satellite recent decades.

For low-cost SUAV; Though being widely used in attitude, sensor fusion techniques confirms; But still there are some problems: at first; The sensor that is used for the attitude estimation in the SUAV system is generally angular rate gyroscope, accelerometer, magnetometer (being used to measure course angle), and the cost of magnetometer is higher, is not suitable for low-cost UAS; Secondly, though some SUAV posture estimation systems adopt GPS to replace magnetometer, reduced cost; But because GPS output is angle; This moment the attitude algorithm for estimating be to be based upon on the basis of Euler's horn cupping, this method need be found the solution trigonometric function in real time in calculating, some the time also singular point can appear; The problem that makes does not have and separates, thereby is not suitable for the relatively poor low-cost flight control computer of computing power.Do not see as yet at present and can solve the solution that attitude is estimated when motor-driven overload appears in unmanned plane preferably.

Summary of the invention

The objective of the invention is shortcoming, a cover attitude heading reference system scheme is provided, only rely on the low-cost sensor that comprises angular rate gyroscope, GPS, accelerometer, realize the estimation of SUAV attitude information to prior art.

The invention provides a kind of attitude heading reference system that is used for low-cost SUAV, comprise angular rate gyroscope, accelerometer, GPS, angular rate gyroscope computing module, correction module and Kalman filter, wherein:

The roll angle speed of angular rate gyroscope survey aircraft, angle of pitch speed and yawrate;

The component of accelerometer measures gravity under the body system of axis;

The flight path azimuthangle of GPS survey aircraft;

Roll angle speed, angle of pitch speed and yawrate that the angular rate gyroscope computing module is measured through angular rate gyroscope obtain the roll angle f with skew ₁, angle of pitch q ₁With crab angle y ₁

Correction module obtains the roll angle f that correction module is estimated through the axon component of the acceleration of gravity of accelerometer measures ₂, angle of pitch q ₂, and the flight path azimuthangle that GPS is measured is as the crab angle y that estimates ₂

Kalman filter merges the data of angular rate gyroscope computing module and correction module generation, obtains final attitude angle [f q y] ^T

Beneficial effect

The invention has the advantages that:, reduced the cost of SUAV system through using low-cost sensor; Reduced the complexity of traditional attitude heading reference system; Rely on sensor fusion techniques, improved precision and reliability that system's boat appearance is estimated; Attitude is estimated inaccurate problem when having solved unmanned plane appearance overload.

Description of drawings

Fig. 1 is the structural drawing of attitude heading reference system;

Fig. 2 resolves for roll angle under the static condition;

Fig. 3 resolves for the angle of pitch under the static condition;

Fig. 4 estimates for the angular rate gyroscope drift;

Fig. 5 is roll angle error and standard deviation thereof;

Fig. 6 is roll angle rate gyro drift error and standard deviation thereof;

Fig. 7 is for estimating that roll angle

is with the error contrast between the true roll angle f;

Fig. 8 estimates that based on the roll angle of flight test wherein (a) estimates roll angle for time window method, (b) is conventional filtering and gyro Integral Estimation roll angle;

Fig. 9 estimates that based on the angle of pitch of flight test wherein (a) estimates the angle of pitch for time window method, (b) is the conventional filtering and the gyro Integral Estimation angle of pitch;

Figure 10 is a filter switch sign amount;

Figure 11 is filter switch triggering amount and threshold value.

Embodiment

Below in conjunction with accompanying drawing, specify preferred implementation of the present invention.

This embodiment has been realized a kind of attitude heading reference system that is used for low-cost SUAV, and this system is made up of angular rate gyroscope subsystem, syndrome system and three parts of Kalman filter.Fig. 1 has shown the structural drawing of attitude heading reference system.This embodiment adopts the error quaternion method to set up model, has alleviated the computational load of airborne processor.

1) angular rate gyroscope computing module

The attitude of rigid body is normally described with three Eulerian angle in the three dimensions, and they are respectively roll angle f, angle of pitch q and crab angle y.Attitude angle has been described the relativeness between two different coordinates.In Navigation, Guidance and Control, for the aircraft of near-earth motion, usually with body coordinate axis S _b-Oxyz system elects moving coordinate system as, and the navigation coordinate axle is S _n-Ox _ny _nz _nElect reference frame as, be defined as east northeast ground (North-East-Down, NED) frame of reference, x _nBe north orientation, y _nBe east orientation, z _nPoint to the earth's core.

Define any vector u, it is expressed as u respectively in the navigation coordinate axle system and the body system of axis ⁿAnd u ^bThe projection conversion of vector u under above-mentioned two different systems of axis is that the direction of passage cosine matrix is realized:

$u^b=C_n^b{u}^n---\left(1\right)$

is direction cosine matrix (Direction Cosine Matrix in the formula; DCM); Be also referred to as attitude matrix, common formal definition with Eulerian angle.

In practical application, use Eulerian angle to represent that attitude exists some problems.At first, at some in particular cases, indivedual attitude angle become uncertain, and singularity appears in kinematical equation, and for example when angle of pitch q=± 90 °, crab angle y is uncertain, and equation dy/dt has singularity.Secondly, in actual resolving, Euler's horn cupping need be found the solution a large amount of trigonometric functions, must bring computation burden to processor.If use the attitude quaternion method, can avoid the singularity of the equation of motion on the one hand, need not to calculate trigonometric function on the other hand, saved processor resource, improved counting yield.In order to overcome above-mentioned shortcoming, this embodiment uses the modeling of hypercomplex number method.

Definition attitude quaternion q:

$q=\left[\begin{array}{c}{q}_0\\ \stackrel{r}{q}\end{array}\right]---\left(2\right)$

Q wherein ₀The scalar part that is called hypercomplex number q,

The vector part that is called hypercomplex number q.Direction cosine battle array with Quaternion Representation defines as follows:

$C_n^b\left(q\right)=\left[\begin{array}{ccc}1-2({\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000051922760000012.png,HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2+{\mathrm{<figure-callout\; id="3"\; label="q"\; filenames="HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout>}}_3^2)& 2(q_1{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000051922760000012.png,HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout>}}_2+q_3{q}_0)& 2(q_1{q}_3-q_2{q}_0)\\ 2(q_1{q}_2-q_3{q}_0)& 1-2({\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA0000051922760000012.png,HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2+{\mathrm{<figure-callout\; id="3"\; label="q"\; filenames="HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout>}}_3^2)& 2(q_2{\mathrm{<figure-callout\; id="3"\; label="q"\; filenames="HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout>}}_3+q_0{q}_1)\\ 2(q_1{\mathrm{<figure-callout\; id="3"\; label="q"\; filenames="HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout>}}_3+q_2{q}_0)& 2(q_2{q}_3-q_1{q}_0)& 1-2({\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA0000051922760000012.png,HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2+{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000051922760000012.png,HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2)\end{array}\right]---\left(3\right)$

The hypercomplex number differential equation is confirmed by following formula:

Wherein, p is a roll angle speed, q angle of pitch speed, and r is a yawrate.

Can know by hypercomplex number differential equation (4), but the measured value real-time update hypercomplex number through angular rate gyroscope, and the direction cosine matrix of representing according to attitude quaternion then (3) obtains the attitude of carrier angle

$\text{f=arctan}\frac{2(q_2{\mathrm{<figure-callout\; id="3"\; label="q"\; filenames="HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout>}}_3+q_0{q}_1)}{1-2({\mathrm{<figure-callout\; id="1"\; label="q"\; filenames="HDA0000051922760000012.png,HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout>}}_1^2+{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000051922760000012.png,HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2)}$

q＝arcsin?2(-q ₁q ₃+q ₂q ₀) (5)

$\text{y=arctan}\frac{2(q_1{\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000051922760000012.png,HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout>}}_2+q_3{q}_0)}{1-2({\mathrm{<figure-callout\; id="2"\; label="q"\; filenames="HDA0000051922760000012.png,HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout>}}_2^2+{\mathrm{<figure-callout\; id="3"\; label="q"\; filenames="HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout>}}_3^2)}$

2) correction module

Although the attitude angle that obtains through angular rate gyroscope measured value integration exists the unbounded error,, thereby be that most of AHRS system is indispensable because angular rate gyroscope can provide the output of high bandwidth.The drift that is produced by the gyro integration can suppress through corrective system, because form the characteristic that the sensor of corrective system possesses output error bounded.Corrective system periodically " replacement " attitude information of providing through angular rate gyroscope to reach the purpose of correction.This embodiment chooses accelerometer and GPS forms corrective system jointly.

Through accelerometer to gravitational vector the projection under the body system of axis observe and can obtain attitude angle, according to the principle of accelerometer, suppose that its output vector of three is f ^b, can represent

$f^b=a_{\mathrm{ib}}^b-g_b---\left(6\right)$

Wherein,

Be acceleration of motion the component under the body system of axis of carrier with respect to inertial system, g _bBe the component of gravity acceleration under the body system of axis

$g_b=C_n^b{g}_n=\left[\begin{array}{c}-g\sin q\\ g\sin f\cos q\\ g\cos f\cos q\end{array}\right]---\left(7\right)$

Wherein, for the projection of gravity acceleration under navigation coordinate axle system, can be expressed as g _n=[0 0 g] ^T, g is the gravity acceleration constant.

The time, promptly carrier is static or when doing linear uniform motion, f ^bBecome

$f^b=\left[\begin{array}{c}{f}_x\\ f_y\\ f_z\end{array}\right]=-g_b=\left[\begin{array}{c}g\sin q\\ -g\sin f\cos q\\ -g\cos f\cos q\end{array}\right]---\left(8\right)$

At this moment, angle of pitch q and roll angle f can confirm through following formula

$q=\arcsin \frac{f_x}{g}$ (9)

$f=\arcsin \frac{-f_y}{g\cos q}$

Formula (8) does not comprise crab angle y, thereby only can only confirm angle of pitch q and roll angle f through accelerometer.In order to confirm crab angle y, also need introduce the course signal that GPS provides.It is pointed out that GPS can only provide flight path azimuthangle but under the less situation of crosswind, can be used for substituting crab angle y.

3) Kalman filter

This embodiment selection card Thalmann filter is as the sensor fusion algorithm that angular rate gyroscope and corrective system are combined.

The wave filter job step is: after confirming starting condition, just can utilize the measurement data integration of angular rate gyroscope, carry out attitude and estimate that this step is called " time renewal ", and the attitude information of high bandwidth is provided.In the time update stage,, make the error of estimating attitude accumulate in time, thereby can not lean on simple gyro data integration to confirm attitude because there is noise in gyro data.In order to suppress gyro error, need to introduce corrective system gyro data is periodically revised, this step is called " measure and upgrade ".The covariance matrix of state error and gyroscopic drift are estimated also to proofread and correct in this step.Next, carry out the time of a new round and upgrade, so periodically repeat.

True attitude quaternion q can be expressed as the estimation hypercomplex number

With error quaternion q _eThe form that multiplies each other is promptly thought error quaternion q _eBe to estimate hypercomplex number

Rotation to true hypercomplex number q.Because the error of angular rate gyroscope, error quaternion is a small amount of of non-zero.q _e,

Confirm by following formula with the relation of q:

$q=\hat{q}\&CircleTimes;q_e---\left(10\right)$

But the error quaternion approximate representation is:

$q_e=\left[\begin{array}{c}1\\ {\stackrel{r}{q}}_e\end{array}\right]---\left(11\right)$

To formula (11) differentiate, and utilize hypercomplex number differential equation (4), can get the hypercomplex number error differential equation through linearization

Wherein,

representes the poor of angular rate gyroscope measured value and actual value,

confirm by following formula

$[\stackrel{r}{\mathrm{\&omega;}}\&times;]=\left[\begin{array}{ccc}0& r& -q\\ -r& 0& p\\ q& -p& 0\end{array}\right]---\left(13\right)$

The observed quantity of syndrome system is taken as error angle; Can obtain by following method: at first utilize the metrical information of accelerometer and GPS to calculate the attitude of carrier angle according to formula (9); The attitude angle estimated value that then this attitude angle cotype (5) is calculated is subtracted each other, and can obtain angular error

$\left[\begin{array}{c}\mathrm{\&delta;f}\\ \mathrm{\&delta;q}\\ \mathrm{\&delta;y}\end{array}\right]=\left[\begin{array}{c}{f}_{\mathrm{AS}}\\ q_{\mathrm{AS}}\\ y_{\mathrm{AS}}\end{array}\right]-\left[\begin{array}{c}\hat{f}\\ \hat{q}\\ \hat{y}\end{array}\right]---\left(14\right)$

In the formula, [δ f δ q δ y] ^TBe error angle, [f _ASq _ASy _AS] ^TBe the attitude angle that obtains through corrective system,

Be the attitude angle of utilizing the hypercomplex number method to calculate.

Angle error is confirmed by following formula with the relation between the attitude quaternion

${\stackrel{r}{q}}_e={\left[\begin{array}{ccc}\frac{\mathrm{\&delta;<figure-callout\; id="2"\; label="f"\; filenames="HDA0000051922760000012.png,HDA0000051922760000031.png"\; state="\{\{state\}\}">f</figure-callout>}}{2}& \frac{\mathrm{\&delta;<figure-callout\; id="2"\; label="q"\; filenames="HDA0000051922760000012.png,HDA0000051922760000031.png"\; state="\{\{state\}\}">q</figure-callout>}}{2}& \frac{\mathrm{\&delta;y}}{2}\end{array}\right]}^T---\left(15\right)$

The quantity of state of syndrome system is chosen for the vector part of error quaternion

Drift δ b with three angular rate gyroscopes _p, δ b _qWith δ b _r

$X={\left[\begin{array}{cccc}{\stackrel{r}{q}}_e& \mathrm{\&delta;}{b}_p& \mathrm{\&delta;}{b}_q& \mathrm{\&delta;}{b}_r\end{array}\right]}^T---\left(16\right)$

State equation that then can tectonic system

Wherein, A (t) is a system matrix, and definition as follows

$A\left(t\right)=\left[\begin{array}{cc}-[\stackrel{r}{\mathrm{\&omega;}}\&times;]& -\frac{1}{2}{I}_{3\&times;3}\\ 0_{3\&times;3}& -\frac{1}{\mathrm{\&tau;}}{I}_{3\&times;3}\end{array}\right]---\left(18\right)$

Input matrix B (t) and noise matrix W (t) definition are as follows

$\begin{array}{cc}B\left(t\right)=\left[\begin{array}{cc}-\frac{1}{2}{I}_{3\&times;3}& 0_{3\&times;3}\\ 0_{3\&times;3}& I_{3\&times;3}\end{array}\right]& W\left(t\right)\left[\begin{array}{c}{b}_w\\ w_{b_1}\end{array}\right]\end{array}---\left(19\right)$

Formula (17) can disperse and turn to following form

X _k+1＝F _kX _k+G _kW _k (20)

Wherein, F _kBe state-transition matrix, G _kFor system noise drives matrix

F _k＝I+A(t)Δt (21)

G _k＝B(t)Δt

Write observation equation as following discrete form

Z _k＝H _kX _k+V _k (22)

Wherein, Z=[δ f δ q δ y] ^TBe observed quantity, V is an observation noise, and H is an observing matrix, can be confirmed by formula (15)

H＝[2I _3×3?0 _3×3](23)

In the measurement update stage of Kalman filtering, proofread and correct through the attitude information that accelerometer and GPS diagonal angle rate gyro system provide.Using the precondition of accelerometer correction gyroscopic drift is that carrier is in static or the linear uniform motion state; Owing to there is not coriolis acceleration; Accelerometer sensitive to have only the component of acceleration of gravity on the body coordinate axis, utilize formula (9) just can calculate angle of pitch q and roll angle f this moment.Yet, when carrier is motor-driven, owing to accelerometer has been experienced coriolis acceleration and caused attitude to estimate to occur deviation.Therefore, must adopt certain compensation method, to reduce the attitude error that carrier occurs when motor-driven.Propose two kinds of solutions below and analyze.

A) overload compensation

When aircraft carries out banked turn, owing to centripetal acceleration occurs, can cause accelerometer measuring error to occur, it is inaccurate to make attitude estimate.For the ease of analyzing, can the coordinate turn of aircraft be seen to move in a circle.

The centripetal acceleration that particle moves in a circle is confirmed by following formula

a＝ω·V (24)

In the formula, ω represents the angular velocity of particle around the center of circle, and V represents particle velocity.Obviously, velocity, angular velocity turning axle and acceleration quadrature.Outside transient process, think that the velocity of aircraft is along body x direction of principal axis.Like this, owing to do not have speed component at body y axle and z direction of principal axis, the axial accelerometer of body x can not be experienced centripetal acceleration.The installation all of angular rate gyroscope and accelerometer is all along the body coordinate axis, and the true air speed V of pitot measurement _TASAlso be axial, so the measurement data of angular rate gyroscope and pitot can directly be used for the corrected acceleration meter along body x.Owing in Kalman filtering algorithm, do not use axial accelerometer a along body z _z, only need here to consider to the axial accelerometer a of body y _yCorrection.

Axial angular speed p of body x axle and z and r can cause the axial acceleration q of body y _yYet, if with the centripetal acceleration formula write as form around the angular velocity and the radius of circle in the center of circle (as shown in the formula) just can find out that if radius is very little, the acceleration that causes so will be very little also.

a＝ω ²R (25)

Owing to the body lift-over that aileron movement causes is always axial along body x, simultaneously, accelerometer a _yWith x direction of principal axis close together.Like this, also can ignore around the axial acceleration of y that the angular velocity p of x axle causes, by the coriolis acceleration that turn to produce fully by around axial yaw rate r of z and true air speed V _TASDecision

a _y＝r·V _TAS (26)

Therefore, just can be write as following form through revised formula (9)

$q=\arcsin \frac{f_x}{g}$ (27)

$f=\arcsin \frac{-f_y+r{V}_{\mathrm{TAS}}}{g\cos q}$

Carry out one type of SUAV of detection mission, the most of the time is in cruising condition, when needs change course, then carries out banked turn, and for this kind unmanned plane, this compensation scheme can be obtained satisfied effect.

B) time window

When aircraft flight, can cruise and motor-driven two states between change.Patrol when flying, can be similar to and think that aircraft is not transshipped, the corrective system of this moment can be used for revising gyroscopic drift.Aircraft is done when motor-driven, and corrective system is unavailable owing to receive coriolis acceleration to disturb.Generally, aircraft is done continuously the motor-driven time can be very not long, during this accumulation of gyro error less, therefore can consider Kalman filter is closed, when finishing when motor-driven, row is opened again.

During aircraft turns; Angular speed can have significant change; Therefore the norm

of selecting angular speed is as the sign that triggers Kalman filter, and the switching of time window is confirmed by following formula

$\left\{\begin{array}{c}u\left(t\right)\&le;\mathrm{\&beta;},\mathrm{flag}=1\\ u\left(t\right)>\mathrm{\&beta;},\mathrm{flag}=0\end{array}\right.---\left(28\right)$

Wherein, β is the time window threshold value, and flag is the window sign.

When u (t)≤β, flag=1, window is opened, and uses Kalman filter to carry out deciding appearance; When u (t)＞β, flag=0, close, break off wave filter this moment, stops the correction of corrective system.

It is following to use the described attitude heading reference system of this embodiment to carry out the practical implementation step that attitude information estimates:

The first step: utilize the angular rate gyroscope measurement data to calculate attitude quaternion

Second step: according to F in the formula (21) and G predicted state error covariance matrix

$P_k^{-}=F_{k-1}{P}_{k-1}^{+}{F}_{k-1}^T+G_{k-1}{Q}_{k-1}{G}_{k-1}^T---\left(30\right)$

In the formula, Q _K-1Be the process noise covariance matrix.

The 3rd step: in corrective system, the course angle y that utilizes (9) and GPS to measure _GPS, obtain the attitude angle information [f that estimates by corrective system _ASq _ASy _AS] ^T

The 4th step: utilize the long system's attitude angle information estimated of a last time step poor, obtain the observed quantity of Kalman filter with the attitude angle information that corrective system obtains

$Z=\left[\begin{array}{c}\mathrm{\&delta;f}\\ \mathrm{\&delta;q}\\ \mathrm{\&delta;y}\end{array}\right]=\left[\begin{array}{c}{f}_{\mathrm{AS}}\\ q_{\mathrm{AS}}\\ y_{\mathrm{AS}}\end{array}\right]-\left[\begin{array}{c}\hat{f}\\ \hat{q}\\ \hat{y}\end{array}\right]---\left(31\right)$

The 5th step: calculate kalman gain matrix

$K_k=P_{k-1}{H}_k^T{(H_k{P}_{k-1}{H}_k^T+R_k)}^{-1}---\left(32\right)$

In the formula, R _kBe the measurement noise covariance matrix.

The 6th step: update mode error covariance matrix

$P_k^{+}=(I-K_k{H}_k)P_k^{-}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T---\left(33\right)$

The 7th step: update mode vector

The 8th step: gained attitude quaternion and angular rate gyroscope measured value in hypercomplex number that use is estimated and the gyroscopic drift correction time step of updating

${\hat{b}}_k^{+}={\hat{b}}_k^{-}+\mathrm{\&delta;}{\hat{b}}_k^{+}---\left(35\right)$

${\widehat{\mathrm{\&omega;}}}_k^{+}={\widehat{\mathrm{\&omega;}}}_k^{-}-{\hat{b}}_k^{+}$

The 9th step: by attitude quaternion

Can calculate attitude angle [f q y] ^T

Through static state experiment and flight test combination property of the present invention is verified below.

1) static experiment

In envelope test, the AHRS-400CC Inertial Measurement Unit (Inertial Measurement Units, IMUs) the auxiliary test of heuristics of accomplishing that utilize Crossbow Technology to produce.AHRS-400CC has three axis accelerometer, angular rate gyroscope and magnetometer, can export lift-over, pitching and three Eulerian angle of driftage through built-in attitude algorithm for estimating.Gyro and accelerometer data that this experiment uses AHRS-400CC to gather carry out attitude algorithm, and the attitude angle that goes out that will calculate compares with " the true attitude " that AHRS-400CC exports.Because the course information that the attitude algorithm that this paper proposes has used GPS to provide is proofreaied and correct crab angle, and AHRS-400CC does not have the GPS sensor, therefore in the static state experiment, only carries out the checking of the angle of pitch and roll angle.The AHRS-400CC horizontal stationary is placed, and pick-up transducers data and attitude angle information, experimental period are 160s.

Fig. 2 and Fig. 3 have shown the result that resolves of attitude angle under the static condition.Solid line is the attitude angle that Kalman filtering algorithm obtains, the attitude angle that dotted line obtains for the gyro integration.Can find out that measure noise owing to exist, the attitude angle of using angular rate gyroscope measured value integration to obtain merely can increase in time and drift about.The drift that roll angle produces (be about 0.0156 °/s) greater than angle of pitch drift (be about 0.0031 °/s), this is because the drift of body x direction of principal axis gyro causes greater than the drift of z direction of principal axis gyro.Attitude angle by Kalman filtering algorithm is estimated is not drifted about, and has less oscillation amplitude.Table 1 has provided root-mean-square error (Root Mean Square Error, RMSE) contrast of three kinds of algorithms.This shows that through the correction of accelerometer, the attitude angle that Kalman filtering algorithm is estimated can effectively suppress gyroscopic drift.

Table 1 attitude angle algorithm for estimating RMSE contrast

To angular rate gyroscope drift b _p, b _qEstimation as shown in Figure 4.Solid line is b _p, dotted line is b _qCan know that by figure the drift of roll angle rate gyro is slightly larger than the drift of angle of pitch rate gyro, this is consistent with Fig. 2 and Fig. 3 result displayed.Gyroscopic drift finally can be stable at normal value.

Fig. 5 and Fig. 6 have shown roll angle error (solid line) δ f, roll angle rate gyro drift error (solid line) the δ b that Kalman filtering algorithm obtains respectively _pAnd corresponding 1 σ standard deviation circle (dotted line).The same with aforementioned analysis, two figure show that the error of Kalman filtering system is a bounded.1 σ standard deviation of the roll angle error of stable state has proved once more that less than the noise criteria of corrective system poor (0.2636 °) adopting the sensor fusion algorithm to carry out attitude angle estimates to be superior to simple angular rate gyroscope system or corrective system.Angle of pitch error is similar with the roll angle error condition, repeats no more here.

Estimate roll angle

With the error e between the true roll angle f _φAs shown in Figure 7.Dotted line is the e that only estimates generation with angular rate gyroscope _φThough noise is very little, it is bigger to drift about.Solid line is the e that Kalman filtering algorithm is estimated generation _φ, visible, the advantage that the attitude angle of being estimated by sensor fusion techniques has combined the low noise corrective system of gyrosystem not drift about.

2) flight test

For the performance of further checking boat appearance system in the Live Flying environment, after mathematical simulation and ground experiment, also need make a flight test.Process of the test is following: commercial Inertial Measurement Unit AHRS-400CC is placed on the carrier unmanned plane, beginning the back record flight data of flying, comprise 3-axis acceleration, three axis angular rates and flight time.For the attitude estimated capacity of checking boat appearance system under the carrier maneuvering condition, carry out big maneuvering flight by controlling the hand operation aircraft, roll angle f variation range is between ± 80 °, angle of pitch q variation range is between ± 40 °.After flight finished, the attitude algorithm for estimating that uses this chapter to propose carried out off-line filtering to the data of AHRS-400CC collection and calculates and analyze, and obtains attitude angle information.At last, the attitude angle comparative analysis that the attitude angle that filtering is obtained is calculated with AHRS-400CC self built-in algorithms.Owing to possess the solution under the carrier maneuvering condition in the AHRS-400CC boat appearance algorithm for estimating, so think that the attitude angle of its output is real aspect.

According to the conclusion of simulation analysis, the overload penalty method is unavailable under big dynamically flying condition, so this joint only adopts time window method to handle flying quality.Fig. 8 and Fig. 9 have shown the contrast of estimating between the together true attitude angle of attitude angle, have adopted three kinds of different attitude angle methods of estimation to compare respectively, comprise time window method (dotted line), gyro integration (line) and conventional Kalman filtering (dotted line).Owing to do not use the GPS module in this flight test, can not proofread and correct crab angle y, only estimated the roll angle f and the angle of pitch q of unmanned plane here.

(a) by Fig. 8 (a) and Fig. 9 can know, the attitude angle that adopts time window method to estimate can meet actual value well.In figure (b), the attitude angle that the gyro integration obtains has certain drift.And the attitude angle distortion that relies on conventional Kalman filtering to calculate is serious; This is because in flight test; The unmanned plane most of the time is in overload, and the metric data of accelerometer can not react attitude information faithfully, thereby it can not correctly be revised the gyro integrated value.Table 2 has provided three kinds of methods and has carried out the RMSE that attitude is estimated.This shows that time window method has solved the attitude estimation problem under the carrier maneuvering condition preferably.

Table 2 is based on the attitude estimated value RMSE contrast of flight test

Figure 10 has provided the sign amount of control filters switch in the attitude estimation procedure, and Figure 11 has then shown triggering amount and the threshold value (among the figure shown in the label 1) of filter switch, triggers to measure here to be yaw rate r.Can find out,, cause bigger yaw rate because the course motion is more violent.When triggering amount during greater than threshold value, the sign amount is zero, and wave filter breaks off, and relies on this moment angular rate gyroscope to estimate attitude fully.When the triggering amount was lower than threshold value, the sign amount put one, started Kalman filter, and can utilize accelerometer correction estimated value because motor-driven overload is very little this moment, confirmed thereby accomplish attitude.

To sum up visible, the present invention proposes a kind of based on the attitude course algorithm for estimating of low-cost sensor (angular rate gyroscope, accelerometer, GPS module).The mathematical model of motion carrier attitude heading reference system based on error quaternion of at first having derived adopts sensor fusion techniques based on Kalman filtering to improve the precision of boat appearance system.For the SUAV in spatial movement, when it was in maneuvering condition, corrective system can not be estimated attitude exactly owing to there is coriolis acceleration.This has proposed two kinds of solutions of time window method and overload penalty method to this problem.After having confirmed attitude heading reference system scheme, static experiment and flight test have been carried out respectively with checking attitude algorithm validity and feasibility.Interpretation of result shows that the present invention can estimate the spatial attitude in the unmanned plane during flying process exactly, can be applied to low-cost SUAV system.

Claims (6)

1. an attitude heading reference system that is used for low-cost SUAV is characterized in that, comprises angular rate gyroscope, accelerometer, GPS, angular rate gyroscope computing module, correction module and Kalman filter, wherein:

The roll angle speed of angular rate gyroscope survey aircraft, angle of pitch speed and yawrate;

The component of accelerometer measures gravity under the body system of axis;

The flight path azimuthangle of GPS survey aircraft;

Roll angle speed, angle of pitch speed and yawrate that the angular rate gyroscope computing module is measured through angular rate gyroscope obtain the roll angle f with skew ₁, angle of pitch q ₁With crab angle y ₁

Correction module obtains the roll angle f that correction module is estimated through the component of gravity under the body system of axis of accelerometer measures ₂, angle of pitch q ₂, and the flight path azimuthangle that GPS is measured is as the crab angle y that estimates ₂

Kalman filter merges the data of angular rate gyroscope computing module and correction module generation, obtains final attitude angle [f q y] ^T

2. attitude heading reference system according to claim 1 is characterized in that, said angular rate gyroscope computing module uses the modeling of hypercomplex number method, at first defines attitude quaternion q:

$q=\left[\begin{array}{c}{q}_0\\ \stackrel{r}{q}\end{array}\right]$

Q wherein ₀The scalar part that is called hypercomplex number q, The vector part that is called hypercomplex number q, through the measured value real-time update hypercomplex number of angular rate gyroscope:

Wherein, p is a roll angle speed, q angle of pitch speed, and r is a yawrate;

Obtain the attitude of carrier angle through following formula then

$f_1=\arctan \frac{2(q_2{q}_3+q_0{q}_1)}{1-2(q_1^2+q_2^2)}$

q ₁＝arcsin2(-q ₁q ₃+q ₂q ₀)。

$y_1=\arctan \frac{2(q_1{q}_2+q_3{q}_0)}{1-2(q_2^2+q_3^2)}$

3. attitude heading reference system according to claim 1 and 2 is characterized in that, said correction module is confirmed the roll angle f of estimation through following formula ₂With angle of pitch q ₂:

$q_2=\arcsin \frac{f_x}{g}$

$f_2=\arcsin \frac{-f_y}{g\cos q}$

Wherein, g is the gravity acceleration constant, f _xBe accelerometer readings.

4. attitude heading reference system according to claim 1 and 2 is characterized in that, the concrete grammar that said Kalman filter is carried out data fusion is:

The first step: utilize the angular rate gyroscope measurement data to calculate attitude quaternion

Second step: according to the formula mistake! Do not find Reference source.In with predicted state error covariance matrix P

$P_k^{-}=F_{k-1}{P}_{k-1}^{+}{F}_{k-1}^T+G_{k-1}{Q}_{k-1}{G}_{k-1}^T$

In the formula, Q _K-1Be the process noise covariance matrix, F and G are Jacobian matrix.

The 3rd step: in corrective system, utilize acceleration to take into account GPS, obtain the attitude angle information [f that estimates by corrective system ₂q ₂y ₂] ^T

The 4th step: utilize the long system's attitude angle information

estimated of a last time step poor, obtain the observed quantity of Kalman filter with the attitude angle information that corrective system obtains

$Z=\left[\begin{array}{c}\mathrm{\&delta;f}\\ \mathrm{\&delta;q}\\ \mathrm{\&delta;y}\end{array}\right]=\left[\begin{array}{c}{f}_2\\ q_2\\ y_2\end{array}\right]-\left[\begin{array}{c}\hat{f}\\ \hat{q}\\ \hat{y}\end{array}\right]$

The 5th step: calculate kalman gain matrix

$K_k=P_{k-1}{H}_k^T{(H_k{P}_{k-1}{H}_k^T+R_k)}^{-1}$

In the formula, R _kBe the measurement noise covariance matrix, H is a Jacobian matrix;

The 6th step: update mode error covariance matrix

$P_k^{+}=(I-K_k{H}_k)P_k^{-}{(I-K_k{H}_k)}^T+K_k{R}_k{K}_k^T$

The 7th step: update mode vector

Eighth step: using the estimated error quaternion

and the gyro drift and

the resulting correction time updating step quaternion

and angular rate gyro measurement value

${\hat{b}}_k^{+}={\hat{b}}_k^{-}+\mathrm{\&delta;}{\hat{b}}_k^{+}$

${\widehat{\mathrm{\&omega;}}}_k^{+}={\widehat{\mathrm{\&omega;}}}_k^{-}-{\hat{b}}_k^{+}$

The 9th step: by attitude quaternion

Can calculate attitude angle [f q y] ^T

5. attitude heading reference system according to claim 1 and 2 is characterized in that, when aircraft carried out banked turn, said correction module was confirmed the roll angle f of estimation through following formula ₂With angle of pitch q ₂

$q_2=\arcsin \frac{f_x}{g}$

$f_2=\arcsin \frac{-f_y+r{V}_{\mathrm{TAS}}}{g\cos q}.$

6. attitude heading reference system according to claim 5; It is characterized in that; When aircraft carries out banked turn; The norm

of selecting angular speed is as the sign that triggers Kalman filter, and the switching of time window is confirmed by following formula

$\left\{\begin{array}{c}u\left(t\right)\&le;\mathrm{\&beta;},\mathrm{flag}=1\\ u\left(t\right)>\mathrm{\&beta;},\mathrm{flag}=0\end{array}\right.$

Wherein, β is the time window threshold value, and flag is the window sign;

When u (t)≤β, flag=1, window is opened, and uses Kalman filter to carry out deciding appearance; When u (t)＞β, flag=0, close, break off wave filter this moment, stops the correction of corrective system.

Images