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

Attitude heading reference system for low-cost small unmanned aerial vehicle Download PDF

Info

Publication number
CN102692225B
CN102692225B CN201110072606.2A CN201110072606A CN102692225B CN 102692225 B CN102692225 B CN 102692225B CN 201110072606 A CN201110072606 A CN 201110072606A CN 102692225 B CN102692225 B CN 102692225B
Authority
CN
China
Prior art keywords
amp
attitude
angular rate
angle
theta
Prior art date
Application number
CN201110072606.2A
Other languages
Chinese (zh)
Other versions
CN102692225A (en
Inventor
杜小菁
李蒙
李怀建
涂海峰
郭康
Original Assignee
北京理工大学
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 北京理工大学 filed Critical 北京理工大学
Priority to CN201110072606.2A priority Critical patent/CN102692225B/en
Publication of CN102692225A publication Critical patent/CN102692225A/en
Application granted granted Critical
Publication of CN102692225B publication Critical patent/CN102692225B/en

Links

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 theta 1, a yaw angle psi 1 and a roll angle fai 1 with offsets; the aiding module is used for estimating an estimated roll angle fai 2 and an estimated pitch angle theta 2 and using the track azimuth measured by the GPS as an estimated yaw angle psi 2; 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 [fai theta psi]<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 for low cost SUAV (small unmanned aerial vehicle)

Technical field

The present invention relates to a kind of attitude heading reference system, particularly a kind of attitude heading reference system for low cost SUAV (small unmanned aerial vehicle) based on sensor fusion techniques.

Background technology

Attitude heading reference system (Attitude Heading Reference System, AHRS), is called for short aviation attitude system, for determining the orientation of motion carrier in space.The attitude information provided by attitude heading reference system can be widely used in Navigation, Guidance and Control.For the unmanned plane of autonomous flight, aviation attitude system is particularly important, and the inner looping that it is control system provides necessary gesture feedback.Usually, the determination of attitude information depends on high-precision angular rate gyroscope, but the defects such as high accuracy gyroscope also exists that cost is high, weight large, complex structure, obviously can not be applicable to low cost Mini-Unmanned Aerial Vehicles.Therefore, under the restriction condition of low cost sensor, for Mini-Unmanned Aerial Vehicles provides the attitude heading reference system that a set of calculated amount is little, reliability is high always to be the problem that domestic and international association area is extensively studied.

Under normal circumstances, the attitude of motion carrier describes by Eulerian angle (φ, θ, ψ).According to the angular speed of carrier movement with the relation between Eulerian angle rate of change, the measured value of angular rate gyroscope can be carried out integration, thus obtain the attitude information of carrier, but this method is for the requirement of Gyro Precision higher (output error is less than 0.1 °/h).Low cost SUAV (small unmanned aerial vehicle) adopts MEMS (Micro Electrical MechanicalSystem) sensor usually, although possess the features such as cost is low, volume is little, the precision of sensors with auxiliary electrode is poor, output error is between 10 ° ~ 100 °/h, larger drift can be produced during integrating gyroscope output valve, thus can not be directly used in Attitude estimation.

Attitude information can not rely on angular rate gyroscope yet, and adopts measurement vector to determine.The ultimate principle of measurement vector determination attitude is adopted to be: with the measurement vector of two not conllinear under two coordinate systems respectively, to be rotated by three times and two coordinate systems are overlapped, then calculate Eulerian angle.Some documents are studied for this gyro free system (Gyro-Free Systems).Such as, using accelerometer, magnetometer and GPS (Global Positioning System) as sensor, obtaining attitude information by solving Wahba problem; Also can pass through multi-antennas GPS, adopt gps carrier phase observations amount to carry out Attitude estimation.But under a lot of actual conditions, this gyro free aviation attitude system can not be used alone.First, gyro free aviation attitude system can not provide high bandwidth attitude information; Secondly, when carrier do specific motor-driven time, comparatively big error can be there is in the attitude information using the method to estimate.Such as when unmanned plane banked turn, accelerometer can responsive to except acceleration of gravity except centripetal acceleration, cause calculating attitude inaccurate; Finally, the attitude information that gyro free aviation attitude system provides comprises larger noise.

For these reasons, the gyro free system that sensor fusion (Sensor Fusion) technology can be used same for angular rate gyroscope accelerometer, magnetic strength to be taken into account the compositions such as GPS merges, and makes them complement one another, learns from other's strong points to offset one's weaknesses.Because angular rate gyroscope can provide high bandwidth to have inclined attitude information, can be used to " smoothly " gyro free system; Gyro free system then for the drift that correction angle rate gyro produces, therefore also becomes corrective system (Aiding System).Like this, after utilizing suitable wave filter the two to be combined, just high bandwidth can be obtained and without inclined attitude information solution.The core of sensor fusion techniques is the wave filter for merging different sensors, and the method using wave filter fusion angle rate gyro and gyro free system to carry out surveying appearance is widely used in the attitude problem identificatioin of satellite recent decades.

For low cost SUAV (small unmanned aerial vehicle), determine although sensor fusion techniques has been widely used in attitude, but still there are some problems: first, angular rate gyroscope, accelerometer, magnetometer (for measuring course angle) is generally for the sensor of Attitude estimation in Mini-Unmanned Aerial Vehicles, and the cost of magnetometer is higher, be not suitable for low-cost unmanned machine system; Secondly, although some SUAV (small unmanned aerial vehicle) posture estimation systems adopt GPS to replace magnetometer, reduce cost, but due to GPS output is angle, Attitude estimation algorithm be now be based upon Euler's horn cupping basis on, this method needs Real-time solution trigonometric function in the calculation, some time also there will be singular point, make problem without solution, be not thus suitable for the low cost flight control computer that computing power is poor.Have not yet to see the solution that can solve the Attitude estimation when motor-driven overload appears in unmanned plane preferably.

Summary of the invention

The object of the invention is the shortcoming for prior art, a set of attitude heading reference system scheme is provided, only rely on the low cost sensor comprising angular rate gyroscope, GPS, accelerometer, realize SUAV (small unmanned aerial vehicle) attitude information and estimate.

The invention provides a kind of attitude heading reference system for low cost SUAV (small unmanned aerial vehicle), 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, pitch rate and yawrate;

The component of accelerometer measures gravity under body axis system;

The flight path azimuthangle of GPS survey aircraft;

Roll angle speed, pitch rate and yawrate that angular rate gyroscope computing module is measured by angular rate gyroscope, obtain the roll angle φ being with skew 1, pitching angle theta 1with crab angle ψ 1;

Correction module obtains the roll angle φ of correction module estimation by the body axle component of the acceleration of gravity of accelerometer measures 2, pitching angle theta 2, and flight path azimuthangle GPS measurement obtained is as the crab angle ψ estimated 2;

The data that angular rate gyroscope computing module and correction module produce merge by Kalman filter, obtain final attitude angle [φ θ ψ] t.

Beneficial effect

The invention has the advantages that: by using low cost sensor, reduce the cost of Mini-Unmanned Aerial Vehicles; Reduce the complexity of traditional attitude heading reference system; Rely on sensor fusion techniques, improve precision and the reliability of the estimation of system boat appearance; Solve Attitude estimation inaccurate problem when transshipping appears in unmanned plane.

Accompanying drawing explanation

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

Fig. 2 is that under static condition, roll angle resolves;

Fig. 3 is that under static condition, the angle of pitch resolves;

Fig. 4 is angular rate gyroscope drift estimate;

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 roll angle with the error contrast between true roll angle φ;

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

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

Figure 10 is filter switch mark amount;

Figure 11 is filter switch triggering amount and threshold value.

Embodiment

Below in conjunction with accompanying drawing, illustrate the preferred embodiment of the present invention.

Present embodiment achieves a kind of attitude heading reference system for low cost SUAV (small unmanned aerial vehicle), and this system is made up of angular rate gyroscope subsystem, syndrome system and Kalman filter three parts.Fig. 1 shows the structural drawing of attitude heading reference system.Present embodiment adopts error quaternion method Modling model, alleviates the computational load of airborne processor.

1) angular rate gyroscope computing module

In three dimensions, the attitude of rigid body normally describes by three Eulerian angle, and they are respectively roll angle φ, pitching angle theta and crab angle ψ.Attitude angle describes the relativeness between two different coordinates.In Navigation, Guidance and Control, for the aircraft of near-earth motion, usually by body coordinate axis S b– Oxyz system elects moving coordinate system as, navigation coordinate axle system S n– Ox ny nz nelect reference frame as, with being defined as east northeast (North-East-Down, NED) frame of reference, x nfor north orientation, y nfor east orientation, z npoint to the earth's core.

Define any vector u, it is expressed as u in navigation coordinate axle system and body axis system nand u b.The projection transform of vector u under above-mentioned two different systems of axis is realized by direction cosine matrix:

u b = C n b u n - - - ( 1 )

In formula for direction cosine matrix (Direction Cosine Matrix, DCM), also referred to as attitude matrix, usually with the formal definition of Eulerian angle.

In actual applications, Eulerian angle are used to represent that attitude also exists some problem.First, at some in particular cases, indivedual attitude angle becomes uncertain, and singularity appears in kinematical equation, and such as, when pitching angle theta=± 90 °, crab angle ψ is uncertain, and equation d ψ/dt has singularity.Secondly, in actual resolving, Euler's horn cupping needs to solve a large amount of trigonometric function, must bring computation burden to processor.If use attitude quaternion method, the singularity of the equation of motion can be avoided on the one hand, on the other hand without the need to calculating trigonometric function, save processor resource, improve counting yield.In order to overcome above-mentioned shortcoming, present embodiment uses Quaternion Method modeling.

Definition attitude quaternion q:

q = q 0 q &RightArrow; - - - ( 2 )

Wherein q 0be called the scalar component of hypercomplex number q, q &RightArrow; = q 1 q 2 q 3 T Be called the vector section of hypercomplex number q.

Be defined as follows with the Direct cosine matrix that hypercomplex number represents:

C n b ( q ) = 1 - 2 ( q 2 2 + q 3 2 ) 2 ( q 1 q 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 ( q 1 2 + q 3 2 ) 2 ( q 2 q 3 + q 0 q 1 ) 2 ( q 1 q 3 + q 2 q 0 ) 2 ( q 2 q 3 - q 1 q 0 ) 1 - 2 ( q 1 2 + q 2 2 ) - - - ( 3 )

Quaternion differential equation is determined by following formula:

q &CenterDot; = 1 2 &Omega; &CenterDot; q = 1 2 0 - p - q - r p 0 r - q q - r 0 p r q - p 0 q 0 q 1 q 2 q 3 - - - ( 4 )

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

From quaternion differential equation formula (4), can real-time update hypercomplex number by the measured value of angular rate gyroscope, then obtain attitude of carrier angle according to the direction cosine matrix (3) that attitude quaternion represents

&phi; = arctan 2 ( q 1 q 3 + q 0 q 1 ) 1 - 2 ( q 1 2 + q 2 2 ) &theta; = arcsin 2 ( - q 1 q 3 + q 2 q 0 ) &psi; = arctan 2 ( q 1 q 2 + q 3 q 0 ) 1 - 2 ( q 2 2 + q 3 2 ) - - - ( 5 )

2) correction module

Although the attitude angle obtained by angular rate gyroscope measured value integration also exists unbounded error, because angular rate gyroscope can provide the output of high bandwidth, because of but most of AHRS system is indispensable.The drift produced by gyro integration can be suppressed by corrective system, because the sensor of composition corrective system is for the characteristic of output error bounded.Corrective system can periodically " replacement " attitude information of providing through angular rate gyroscope to reach the object of correction.Present embodiment chooses accelerometer and GPS forms corrective system jointly.

By accelerometer, observation is carried out to the projection of gravitational vector under body axis system and can obtain attitude angle, according to the principle of accelerometer, suppose that the output vector of its three axle is f b, can represent

f b = a ib b - g b - - - ( 6 )

Wherein, for carrier is relative to the component of acceleration of motion under body axis system of inertial system, g bfor the component of gravity acceleration under body axis system

g b = C n b g n = - g sin &theta; g sin &phi; cos &theta; g cos &phi; cos &theta; - - - ( 7 )

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

? time, i.e. carrier stationary or when doing linear uniform motion, f bbecome

f b = f x f y f z = - g b = g sin &theta; - g sin &phi; cos &theta; - g cos &phi; cos &theta; - - - ( 8 )

Now, pitching angle theta and roll angle φ can be determined by above formula

&theta; = arcsin f x g

(9)

&phi; = arcsin - f y g cos &theta;

Formula (8) does not comprise crab angle ψ, thus can only determine pitching angle theta and roll angle φ by means of only accelerometer.In order to determine crab angle ψ, also need the course signal that introducing GPS provides.It is pointed out that GPS can only provide flight path azimuthangle but alternative crab angle ψ can be used for when crosswind is less.

3) Kalman filter

Present embodiment selection card Thalmann filter is as sensor fusion algorithm angular rate gyroscope and corrective system combined.

Wave filter job step is: after determining starting condition, just can utilize the measurement data integration of angular rate gyroscope, carry out Attitude estimation, and this step is called " time renewal ", provides the attitude information of high bandwidth.In more new stage time, because gyro data exists noise, make to estimate that the error of attitude is accumulated in time, thus can not lean on simple gyro data integration determination attitude.In order to suppress gyro error, needing to introduce corrective system and periodically revising gyro data, this step is called " measure and upgrade ".The covariance matrix of state error and gyroscopic drift are estimated also to correct in this step.Next, the time of carrying out a new round upgrades, and so periodically repeats.

True attitude quaternion q can be expressed as estimation hypercomplex number with error quaternion q ethe form be multiplied, namely thinks error quaternion q eestimate hypercomplex number to the rotation of true hypercomplex number q.Due to the error of angular rate gyroscope, error quaternion is a small amount of of non-zero.Q e, determined by following formula with the relation of q:

q = q ^ &CircleTimes; q e - - - ( 10 )

Error quaternion can approximate representation be:

q e = 1 q &RightArrow; e - - - ( 11 )

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

q &RightArrow; &CenterDot; e = - [ &omega; &RightArrow; &times; ] q &RightArrow; e - 1 2 ( &omega; ^ - &omega; ) - - - ( 12 )

Wherein, represent the difference of angular rate gyroscope measured value and actual value, determined by following formula

[ &omega; &RightArrow; &times; ] = 0 r - q - r 0 p q - p 0 - - - ( 13 )

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

&delta;&phi; &delta;&theta; &delta;&psi; = &phi; AS &theta; AS &psi; AS - &phi; ^ &theta; ^ &psi; ^ - - - ( 14 )

In formula, [δ φ δ θ δ ψ] tfor error angle, [φ aSθ aSψ aS] tfor the attitude angle obtained by corrective system, &phi; ^ &theta; ^ &psi; ^ T The attitude angle calculated for utilizing Quaternion Method.

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

q &RightArrow; e = &delta;&phi; 2 &delta;&theta; 2 &delta;&psi; 2 T - - - ( 15 )

The quantity of state of syndrome system is chosen for the vector section of error quaternion with the drift δ b of three angular rate gyroscopes p, δ b qwith δ b r

X = q &RightArrow; e &delta;b p &delta;b q &delta;b r T - - - ( 16 )

Then can the state equation of tectonic system

X &CenterDot; ( t ) = A ( t ) X ( t ) + B ( t ) W ( t ) - - - ( 17 )

Wherein, A (t) is system matrix, is defined as follows

A ( t ) = - [ &omega; &RightArrow; &times; ] - 1 2 I 3 &times; 3 0 3 &times; 3 - 1 &tau; I 3 &times; 3 - - - ( 18 )

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

B ( t ) = - 1 2 I 3 &times; 3 0 3 &times; 3 0 3 &times; 3 I 3 &times; 3 W ( t ) b w w b 1 - - - ( 19 )

Formula (17) discretely can turn to following form

X k+1=F kX k+G kW k(20)

Wherein, F kfor 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=[δ φ δ θ δ ψ] tfor observed quantity, V is observation noise, and H is observing matrix, can be determined by formula (15)

H=[2I 3×30 3×3] (23)

In the measurement more new stage of Kalman filtering, the attitude information provided by accelerometer and GPS diagonal angle rate gyro system is corrected.The precondition using accelerometer correction gyroscopic drift is that carrier is in static or linear uniform motion state, owing to there is not coriolis acceleration, accelerometer sensitive to only have the component of acceleration of gravity in body coordinate axis, now utilize formula (9) just can calculate pitching angle theta and roll angle φ.But, when carrier is motor-driven, experiences coriolis acceleration due to accelerometer and caused Attitude estimation to occur deviation.Therefore, certain compensation method must be adopted, with reduce carrier motor-driven time occur attitude error.Propose two kinds of solutions below and analyze.

A) overload compensates

When aircraft carries out banked turn, owing to there is centripetal acceleration, accelerometer can be caused to occur measuring error, make Attitude estimation inaccurate.For the ease of analyzing, the coordinate turn of aircraft can be seen and moving in a circle.

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

a=ω·V (24)

In 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 are orthogonal.Outside transient process, think that the velocity of aircraft is along body x-axis direction.Like this, owing to there is not speed component in body y-axis and z-axis direction, the accelerometer in body x-axis direction can not experience centripetal acceleration.The installation all of angular rate gyroscope and accelerometer is all along body coordinate axis, and the true air speed V that pitot is measured tASalso along body x-axis direction, therefore the measurement data of angular rate gyroscope and pitot directly can be used for corrected acceleration meter.Owing to not using the accelerometer a along body z-axis direction in Kalman filtering algorithm z, only need consider the accelerometer a to body y-axis direction here ycorrection.

Angular speed p and r in body x-axis and z-axis direction can cause the acceleration a in body y-axis direction y.But, if the form of angular velocity centripetal acceleration formula write as around the center of circle and radius of circle (as shown in the formula) just can find out, if radius is very little, the acceleration so caused also will be very little.

a=ω 2R (25)

The body rolling caused due to aileron movement always along body x-axis direction, meanwhile, accelerometer a ywith x-axis direction close together.Like this, the acceleration around the y-axis direction that the angular velocity p of x-axis causes is also negligible, and the coriolis acceleration produced by turning is completely by the yaw rate r around z-axis direction and true air speed V tASdetermine

a y=r·V TAS(26)

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

&theta; = arcsin f x g

(27)

&phi; = arcsin - f y + rV TAS g cos &theta;

Perform a class SUAV (small unmanned aerial vehicle) of detection mission, the most of the time is in cruising condition, then carries out banked turn when needs change course, and for this kind of unmanned plane, this compensation scheme can obtain satisfied effect.

B) time window

When aircraft flight, can cruise and change between motor-driven two states.Patrol when flying, can be similar to and think that aircraft is not transshipped, corrective system now can be used for revising gyroscopic drift.When aircraft does motor-driven, corrective system due to be subject to coriolis acceleration interference and unavailable.Under normal circumstances, aircraft does continuously the motor-driven time can not be very long, and during this, the accumulation of gyro error is less, and Kalman filter therefore can be considered to close, and when terminating motor-driven, then row is opened.

During aircraft turn, angular speed can have significant change, and therefore selects the norm of angular speed as the mark triggering Kalman filter, the opening and closing of time window is determined by following formula

u ( t ) &le; &beta; , flag = 1 u ( t ) > &beta; , flag = 0 - - - ( 28 )

Wherein, β is time window threshold value, and flag is window mark.

As u (t)≤β, flag=1, window is opened, and uses Kalman filter to carry out determining appearance; As u (t) > β, flag=0, close, now disconnects wave filter, stops the correction of corrective system.

The concrete implementation step that the attitude heading reference system of use described in present embodiment carries out attitude information estimation is as follows:

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

q ^ &CenterDot; - = 1 2 &Omega; &CenterDot; q ^ - - - - ( 29 )

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

P k - = F k - 1 P k - 1 + F k - 1 T + G k - 1 Q k - 1 G k - 1 T - - - ( 30 )

In formula, Q k-1for process noise covariance matrix.

3rd step: in corrective system, utilizes the course angle ψ that (9) and GPS measure gPS, obtain the attitude angle information [φ estimated by corrective system aSθ aSψ aS] t.

4th step: utilize the posture angle information that a upper time step is estimated &phi; ^ &theta; ^ &Psi; ^ T The attitude angle information obtained with corrective system is poor, obtains the observed quantity of Kalman filter

Z = &delta;&phi; &delta;&theta; &delta;&psi; = &phi; 2 &theta; 2 &psi; 2 - &phi; ^ &theta; ^ &psi; ^ - - - ( 31 )

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 - - - ( 32 )

In formula, R kfor measurement noise covariance matrix.

6th step: upgrade state 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 - - - ( 33 )

7th step: upgrade state vector

X ^ k + = q &RightArrow; ^ ek + &delta; b ^ pk + &delta; b ^ qk + &delta; b ^ rk + T = K k Z k - - - ( 34 )

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

q ^ k + = q ^ k - &CircleTimes; 1 q &RightArrow; ^ ek + T

b ^ k + = b ^ k - + &delta; b ^ k + - - - ( 35 )

&omega; ^ k + = &omega; ^ k - - b ^ k +

9th step: by attitude quaternion attitude angle [φ θ ψ] can be calculated t.

Below by static experiment and flight test, combination property of the present invention is verified.

1) static experiment

In envelope test, the AHRS-400CC Inertial Measurement Unit (Inertial Measurement Units, IMUs) utilizing Crossbow Technology to produce has assisted test of heuristics.AHRS-400CC has three axis accelerometer, angular rate gyroscope and magnetometer, can export rolling, pitching and driftage three Eulerian angle by built-in Attitude estimation algorithm.The gyro that this experiment uses AHRS-400CC to gather and accelerometer data carry out attitude algorithm, and " the true attitude " that the attitude angle gone out calculated exports with AHRS-400CC are contrasted.Correct crab angle because Attitude Algorithm in this paper employs the course information that GPS provides, and AHRS-400CC does not have GPS sensor, in static experiment, therefore only carry out the checking of the angle of pitch and roll angle.AHRS-400CC horizontal stationary placed, pick-up transducers data and attitude angle information, experimental period is 160s.

Fig. 2 and Fig. 3 shows the calculation result of attitude angle under static condition.Solid line is the attitude angle that Kalman filtering algorithm obtains, and dotted line is the attitude angle that gyro integration obtains.Can find out, owing to there is measurement noises, the simple attitude angle using angular rate gyroscope measured value integration to obtain can increase in time and drift about.The drift (being about 0.0156 °/s) that roll angle produces is greater than angle of pitch drift (being about 0.0031 °/s), and this is that the drift being greater than z-axis direction gyro due to the drift of body x-axis direction gyro causes.The attitude angle estimated by Kalman filtering algorithm is not drifted about, and has less oscillation amplitude.Table 1 gives root-mean-square error (Root Mean Square Error, the RMSE) contrast of three kinds of algorithms.As can be seen here, through the correction of accelerometer, the attitude angle that Kalman filtering algorithm is estimated effectively can suppress gyroscopic drift.

Table 1 pose estimation algorithm RMSE contrasts

To angular rate gyroscope drift b p, b qestimation as shown in Figure 4.Solid line is b p, dotted line is b q.As seen from the figure, the drift of roll angle rate gyro is slightly larger than the drift of pitch rate gyro, and this is consistent with the result that Fig. 2 and Fig. 3 shows.Gyroscopic drift finally can be stable at constant value.

Fig. 5 and Fig. 6 respectively illustrates roll angle error (solid line) δ φ, roll angle rate gyro drift error (solid line) the δ b that Kalman filtering algorithm obtains pand corresponding 1 σ standard deviation circle (dotted line).The same with Such analysis, two figure show that the error of Kalman filtering system is bounded.1 σ standard deviation of the roll angle error of stable state is less than the noise criteria difference (0.2636 °) of corrective system, again demonstrates and adopts sensor fusion algorithm to carry out pose estimation and be better than simple angular rate gyroscope system or corrective system.Angle of pitch error is similar with roll angle error condition, repeats no more here.

Estimate roll angle with the error e between true roll angle φ φas shown in Figure 7.Dotted line is the e only estimating generation with angular rate gyroscope φalthough noise is very little, drift about larger.Solid line is the e that Kalman filtering algorithm is estimated to produce φ, visible, the advantage that the corrective system that the attitude angle estimated by sensor fusion techniques combines gyrosystem low noise is not drifted about.

2) flight test

In order to verify the performance of aviation attitude system in Live Flying environment further, after mathematical simulation and ground experiment, also need to make a flight test.Process of the test is as follows: be placed on carrier unmanned plane by commercial Inertial Measurement Unit AHRS-400CC, and record flight data after starting to fly, comprises 3-axis acceleration, three axis angular rates and flight time.For the Attitude estimation ability of checking aviation attitude system under carrier maneuvering condition, carry out high maneuver flight by manipulation hand operation aircraft, roll angle φ variation range is between ± 80 °, and pitching angle theta variation range is between ± 40 °.After flight terminates, the Attitude estimation algorithm using this chapter to propose carries out off-line filtering computation and analysis to the data that AHRS-400CC gathers, and obtains attitude angle information.Finally, the attitude angle comparative analysis that attitude angle filtering obtained calculates with AHRS-400CC self built-in algorithms.Owing to possessing the solution under carrier maneuvering condition, so think that its attitude angle exported is real aspect in AHRS-400CC boat appearance algorithm for estimating.

According to the conclusion of simulation analysis, penalty method of transshipping under Larger Dynamic flying condition is unavailable, and therefore this section only adopts time window method process flying quality.Fig. 8 and Fig. 9 shows and estimates that attitude angle is with the contrast between true attitude angle, have employed three kinds of different pose estimation methods respectively to contrast, comprise time window method (dotted line), gyro integration (line) and conventional Kalman filtering (dotted line).Owing to not using GPS module in this flight test, can not correct crab angle ψ, only have estimated roll angle φ and the pitching angle theta of unmanned plane here.

From (a) of Fig. 8 (a) and Fig. 9, the attitude angle adopting time window method to estimate can meet actual value well.In figure (b), the attitude angle that gyro integration obtains has certain drift.And the attitude angle distortion relying 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, thus makes it correctly not revise gyro integrated value.Table 2 gives the RMSE that three kinds of methods carry out Attitude estimation.As can be seen here, time window method solves the Attitude estimation problem under carrier maneuvering condition preferably.

Table 2 contrasts based on the Attitude estimation value RMSE of flight test

Figure 10 gives the mark amount controlling filter switch in Attitude estimation process, and Figure 11 then shows triggering amount and the threshold value (shown in number in the figure 1) of filter switch, triggers here and measures as yaw rate r.Can find out, because course motion is relatively more violent, cause larger yaw rate.When triggering amount is greater than threshold value, mark amount is zero, and wave filter disconnects, and now relies on angular rate gyroscope to estimate attitude completely.When triggering amount is lower than threshold value, mark amount puts one, starts Kalman filter, now because motor-driven overload is very little, can utilize accelerometer correction estimated value, thus complete attitude and determine.

As fully visible, the attitude course estimation algorithm that the present invention proposes a kind of based on low cost sensor (angular rate gyroscope, accelerometer, GPS module).First motion carrier attitude of having derived heading reference system, based on the mathematical model of error quaternion, adopts sensor fusion techniques based on Kalman filtering to improve the precision of aviation attitude system.For the SUAV (small unmanned aerial vehicle) in spatial movement, when it is in maneuvering condition, corrective system can not estimate attitude exactly owing to there is coriolis acceleration.This is to this problem, proposes time window method and overload penalty method two kinds of solutions.After determining attitude heading reference system scheme, static experiment and flight test are carried out respectively to verify correctness and the feasibility of Attitude Algorithm.Interpretation of result shows, the present invention can estimate the spatial attitude in unmanned plane during flying process exactly, can be applied to low cost Mini-Unmanned Aerial Vehicles.

Claims (4)

1. for an attitude heading reference system for low cost SUAV (small unmanned aerial vehicle), it is characterized in that, 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, pitch rate and yawrate;
The component of accelerometer measures gravity under body axis system;
The flight path azimuthangle of GPS survey aircraft;
Roll angle speed, pitch rate and yawrate that angular rate gyroscope computing module is measured by angular rate gyroscope, obtain the roll angle φ being with skew 1, pitching angle theta 1with crab angle ψ 1;
Correction module obtains the roll angle φ of correction module estimation by the component of gravity under body axis system of accelerometer measures 2, pitching angle theta 2, and flight path azimuthangle GPS measurement obtained is as the crab angle ψ estimated 2;
The data that angular rate gyroscope computing module and correction module produce merge by Kalman filter, obtain final attitude angle [φ θ ψ] t;
Described angular rate gyroscope computing module uses Quaternion Method modeling, first defines attitude quaternion q:
q = q 0 q &RightArrow;
Wherein q 0be called the scalar component of hypercomplex number q, q &RightArrow; = q 1 q 2 q 3 T Be called the vector section of hypercomplex number q, the measured value real-time update hypercomplex number by angular rate gyroscope:
q &CenterDot; = 1 2 &Omega; &CenterDot; q = 1 2 0 - p - q - r p 0 r - q q - r 0 p r q - p 0 q 0 q 1 q 2 q 3
Wherein, p is roll angle speed, q pitch rate, and r is yawrate;
Then attitude of carrier angle is obtained by following formula
&phi; 1 = arctan 2 ( q 1 q 3 + q 0 q 1 ) 1 - 2 ( q 1 2 + q 2 2 ) &theta; 1 = arcsin 2 ( - q 1 q 3 + q 2 q 0 ) &psi; 1 = arctan 2 ( q 1 q 2 + q 3 q 0 ) 1 - 2 ( q 2 2 + q 3 2 ) ;
Described Kalman filter job step is: after determining starting condition, just utilizes the measurement data integration of angular rate gyroscope, carries out Attitude estimation, and this step is called " time renewal ", provides the attitude information of high bandwidth; In more new stage time, because gyro data exists noise, make to estimate that the error of attitude is accumulated in time, thus simple gyro data integration determination attitude can not be leaned on, in order to suppress gyro error, need to introduce corrective system periodically to revise gyro data, this step is called " measure and upgrade ", and the covariance matrix of state error and gyroscopic drift are estimated also to correct in this step; Next, the time of carrying out a new round upgrades, and so periodically repeats;
The concrete grammar that described Kalman filter carries out data fusion is:
The first step: utilize angular rate gyroscope measurement data to calculate attitude quaternion
q ^ &CenterDot; - = 1 2 &Omega; &CenterDot; q ^ -
Second step: predicted state error co-variance 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 formula, Q k-1for process noise covariance matrix, F and G is Jacobian matrix;
3rd step: in corrective system, utilizes acceleration to take into account GPS, obtains the attitude angle information [φ estimated by corrective system 2θ 2ψ 2] t;
4th step: utilize the posture angle information that a upper time step is estimated &phi; ^ &theta; ^ &Psi; ^ T The attitude angle information obtained with corrective system is poor, obtains the observed quantity of Kalman filter
Z = &delta;&phi; &delta;&theta; &delta;&psi; = &phi; 2 &theta; 2 &psi; 2 - &phi; ^ &theta; ^ &psi; ^
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 formula, R kfor measurement noise covariance matrix, H is Jacobian matrix;
6th step: upgrade state 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
7th step: upgrade state vector
X ^ k + = q &RightArrow; ^ ek + &delta; b ^ pk + &delta; b ^ qk + &delta; b ^ rk + T = K k Z k
8th step: use the error quaternion estimated and gyroscopic drift with gained attitude quaternion in correction time step of updating and angular rate gyroscope measured value
q ^ k + = q ^ k - &CircleTimes; 1 q &RightArrow; ^ ek + T
b ^ k + = b ^ k - + &delta; b ^ k +
&omega; ^ k + = &omega; ^ k - - b ^ k +
9th step: by attitude quaternion attitude angle [φ θ ψ] can be calculated t.
2. attitude heading reference system according to claim 1, is characterized in that, described correction module determines the roll angle φ estimated by following formula 2and pitching angle theta 2:
&theta; 2 = arcsin f x g
&phi; 2 = arcsin - f y g cos &theta;
Wherein, g is gravity acceleration constant, f x, f yfor accelerometer readings.
3. attitude heading reference system according to claim 1, is characterized in that, when aircraft carries out banked turn, described correction module determines the roll angle φ estimated by following formula 2and pitching angle theta 2
&theta; 2 = arcsin f x g
&phi; 2 = arcsin - f y + rV TAS g cos &theta;
Wherein, g is gravity acceleration constant, f x, f yfor accelerometer readings, r is the yaw rate around z-axis direction, V tASfor true air speed.
4. the attitude heading reference system according to any one of claim 1-3, is characterized in that, when aircraft carries out banked turn, the norm of selection angular speed and absolute value are as the mark triggering Kalman filter, and the opening and closing of time window is determined by following formula
u ( t ) &le; &beta; , flag = 1 u ( t ) > &beta; , flag = 0
Wherein, β is time window threshold value, and flag is window mark;
As u (t)≤β, flag=1, window is opened, and uses Kalman filter to carry out determining appearance; As u (t) > β, flag=0, close, now disconnects wave filter, stops the correction of corrective system.
CN201110072606.2A 2011-03-24 2011-03-24 Attitude heading reference system for low-cost small unmanned aerial vehicle CN102692225B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201110072606.2A CN102692225B (en) 2011-03-24 2011-03-24 Attitude heading reference system for low-cost small unmanned aerial vehicle

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201110072606.2A CN102692225B (en) 2011-03-24 2011-03-24 Attitude heading reference system for low-cost small unmanned aerial vehicle

Publications (2)

Publication Number Publication Date
CN102692225A CN102692225A (en) 2012-09-26
CN102692225B true CN102692225B (en) 2015-03-11

Family

ID=46857836

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201110072606.2A CN102692225B (en) 2011-03-24 2011-03-24 Attitude heading reference system for low-cost small unmanned aerial vehicle

Country Status (1)

Country Link
CN (1) CN102692225B (en)

Families Citing this family (23)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102901484B (en) 2012-10-18 2014-07-23 毕诗捷 Antenna gesture detection sensor and antenna gesture detection method
CN103090870B (en) * 2013-01-21 2015-07-01 西北工业大学 Spacecraft attitude measurement method based on MEMS (micro-electromechanical systems) sensor
CN103345157A (en) * 2013-06-21 2013-10-09 南京航空航天大学 Unmanned aerial vehicle three freedom degree model building method
CN103471594B (en) * 2013-09-24 2016-01-20 成都市星达通科技有限公司 Based on the fine alignment algorithm of AHRS
CN103499348B (en) * 2013-09-24 2016-03-30 成都市星达通科技有限公司 AHRS high-precision attitude method for computing data
CN103712598B (en) * 2013-12-31 2014-12-17 渤海大学 Attitude determination method of small unmanned aerial vehicle
US10073453B2 (en) 2014-01-31 2018-09-11 Flir Systems, Inc. Autopilot autorelease systems and methods
WO2015126678A1 (en) * 2014-02-20 2015-08-27 Flir Systems, Inc. Acceleration corrected attitude estimation systems and methods
CN104075710B (en) * 2014-04-28 2016-09-21 中国科学院光电技术研究所 A kind of motor-driven Extended target based on Trajectory Prediction axial attitude real-time estimation method
CN103954289B (en) * 2014-05-20 2016-06-22 哈尔滨工业大学 The quick motor-driven attitude determination method of a kind of Optical Imaging Satellite
US9880021B2 (en) 2014-10-20 2018-01-30 Honeywell International Inc. Systems and methods for attitude fault detection in one or more inertial measurement units
CN104503466B (en) * 2015-01-05 2017-09-12 北京健德乾坤导航系统科技有限责任公司 A kind of Small and micro-satellite guider
CN104880189B (en) * 2015-05-12 2019-01-29 西安克拉克通信科技有限公司 A kind of antenna for satellite communication in motion low cost tracking anti-interference method
CN104819717B (en) * 2015-05-20 2018-04-24 苏州联芯威电子有限公司 A kind of multi-rotor aerocraft attitude detecting method based on MEMS inertial sensor group
CN105094138A (en) * 2015-07-15 2015-11-25 东北农业大学 Low-altitude autonomous navigation system for rotary-wing unmanned plane
CN105509689B (en) * 2015-11-10 2018-01-05 中国航天空气动力技术研究院 A kind of three axis calibration methods for unmanned aerial vehicle onboard arm discharge
CN107305393A (en) * 2016-04-20 2017-10-31 比亚迪股份有限公司 Unmanned plane and its control method
CN107368087A (en) * 2016-05-13 2017-11-21 威海明达创新科技有限公司 Miniature four-axle aircraft and its control method
CN106482734A (en) * 2016-09-28 2017-03-08 湖南优象科技有限公司 A kind of filtering method for IMU Fusion
CN107426687B (en) * 2017-04-28 2019-08-09 重庆邮电大学 Towards the method for adaptive kalman filtering for merging positioning in the room WiFi/PDR
CN107014386B (en) * 2017-06-02 2019-08-30 武汉云衡智能科技有限公司 A kind of disturbing acceleration measurement method that attitude of flight vehicle resolves
WO2019205152A1 (en) * 2018-04-28 2019-10-31 深圳市大疆创新科技有限公司 Cradle head control method and cradle head
CN109491402A (en) * 2018-11-01 2019-03-19 中国科学技术大学 Multiple no-manned plane based on clustered control cooperates with targeted surveillance control method

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101033973A (en) * 2007-04-10 2007-09-12 南京航空航天大学 Attitude determination method of mini-aircraft inertial integrated navigation system
CN101598557A (en) * 2009-07-15 2009-12-09 北京航空航天大学 Integrated navigation system applied to pilotless aircraft

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101033973A (en) * 2007-04-10 2007-09-12 南京航空航天大学 Attitude determination method of mini-aircraft inertial integrated navigation system
CN101598557A (en) * 2009-07-15 2009-12-09 北京航空航天大学 Integrated navigation system applied to pilotless aircraft

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
《基于MEMS的微型无人机姿态仪的设计》;赵鹏等;《火力与指挥控制》;20090630;第34卷(第6期);第164-167页 *

Also Published As

Publication number Publication date
CN102692225A (en) 2012-09-26

Similar Documents

Publication Publication Date Title
Hong Fuzzy logic based closed-loop strapdown attitude system for unmanned aerial vehicle (UAV)
US7979231B2 (en) Method and system for estimation of inertial sensor errors in remote inertial measurement unit
Gebre-Egziabher et al. Design of multi-sensor attitude determination systems
Gebre-Egziabher et al. A gyro-free quaternion-based attitude determination system suitable for implementation using low cost sensors
Wu et al. Velocity/position integration formula part I: Application to in-flight coarse alignment
Kingston et al. Real-time attitude and position estimation for small UAVs using low-cost sensors
Wu et al. Fast complementary filter for attitude estimation using low-cost MARG sensors
George et al. Tightly coupled INS/GPS with bias estimation for UAV applications
Bryson et al. Vehicle model aided inertial navigation for a UAV using low-cost sensors
CN100356139C (en) Miniature assembled gesture measuring system for mini-satellite
CN100585602C (en) Inertial measuring system error model demonstration test method
CN101413800A (en) Navigating and steady aiming method of navigation / steady aiming integrated system
Cheviron et al. Robust nonlinear fusion of inertial and visual data for position, velocity and attitude estimation of UAV
Hall et al. Quaternion attitude estimation for miniature air vehicles using a multiplicative extended Kalman filter
Johansen et al. On estimation of wind velocity, angle-of-attack and sideslip angle of small UAVs using standard sensors
CN101893445B (en) Rapid initial alignment method for low-accuracy strapdown inertial navigation system under swinging condition
CN105190237A (en) Heading confidence interval estimation
Gebre-Egziabher et al. MAV attitude determination by vector matching
Grip et al. Nonlinear observer for GNSS-aided inertial navigation with quaternion-based attitude estimation
Fakharian et al. Adaptive Kalman filtering based navigation: An IMU/GPS integration approach
CN102252689A (en) Electronic compass calibration method based on magnetic sensor
CN102692225A (en) Attitude heading reference system for low-cost small unmanned aerial vehicle
Sun et al. MEMS-based rotary strapdown inertial navigation system
Pflimlin et al. Nonlinear attitude and gyroscope's bias estimation for a VTOL UAV
Stančić et al. The integration of strap-down INS and GPS based on adaptive error damping

Legal Events

Date Code Title Description
PB01 Publication
C06 Publication
SE01 Entry into force of request for substantive examination
C10 Entry into substantive examination
GR01 Patent grant
C14 Grant of patent or utility model
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20150311

Termination date: 20180324

CF01 Termination of patent right due to non-payment of annual fee