CN101033973B - Attitude determination method of mini-aircraft inertial integrated navigation system - Google Patents

Abstract

This invention relates to a kind of attitude definition method of micro inertia mixed navigation system of microminiature aero craft, belongs to microminiature vehicle attitude definition method. The method utilize guidance information, control Kalman filter noise matrices, to realize self-adapting adjust according to flight status; through parameter adjust to realize attitude definition of micro inertia mixed navigation system on dynamic airborne conditions; the specific process: through GNC closed loop circuit guidance algorithm, to gain aero craft barrel angle; through sensor picking, obtain vehicle rate and specific force; by strap inertial navigation algorithm to resolve out vehicle attitude, velocity and position information; by accelerometer and GPS data to count pose and position measurement information of aero craft; real-time compute horizontal attitude variance of Kalman filter observation noise matrices, through Kalman filter to estimate error of strap inertial guidance system; Amend above acquired pose, velocity and position information, and took as feedback information to import in control system.

Description

The attitude of mini-aircraft inertial integrated navigation system is determined method

One, technical field

The invention belongs to the movement parameter measurement systems technology field of micro air vehicle.

Two, background technology

Micro air vehicle is in closely military surveillance, target search, and there are special use value and wide prospect in fields such as disaster monitoring.The Core Feature of the closed loop that the Navigation, Guide and Controlling system of micro air vehicle forms is that the state of minute vehicle is measured and controlled, and makes it can reach the purpose of autonomous flight.The micro air vehicle attitude and heading reference system is the subsystem of miniature navigation system, is used for the attitude of real-time measurement minute vehicle, and the attitude information of measuring is fed back to the controller of minute vehicle.The micro air vehicle attitude and heading reference system is one of gordian technique in the micro air vehicle for attitude stabilization and flight control algorithm provide the attitude information of micro air vehicle, is the essential condition of micro air vehicle automated spacecraft.

Micro air vehicle inertia assembled gesture determines that system adopts the miniature strapdown inertial navigation system of MEMS (MicroElectro-Mechanical System) inertial sensor (little gyro, micro-acceleration gauge).Adopting the silicon micro-gyroscope and the accelerometer volume of MEMS technology little, in light weight, is present unique inertial sensor that satisfies minute vehicle weight and volume requirement.Strapdown inertial navigation system (Strapdown Inertial Navigation System) is a kind of system that realizes navigation feature based on Newton's laws of motion in the mode of calculating, its core sensor is the measuring unit that is made of gyroscope (angular motion sensor) and accelerometer two class inertial sensors such as (line motion sensors), and the measuring unit that is made of above-mentioned two class sensors directly connects firmly on motion carrier.Processor in the strapdown inertial navigation system is by A/D conversion circuit or directly adopt digital interface, read the data of interior angular motion of above-mentioned measuring unit and line motion sensor, press the principle process of inertial navigation algorithm, primary measuring data is processed, calculate the parameters such as attitude, speed and position of motion carrier.

MEMS inertial sensor measuring error is big, can disperse fast based on the attitude error of the micro-inertial navigation system of MEMS inertial sensor, is difficult to satisfy the demand of micro air vehicle autonomous flight.Therefore in the miniature inertial navigation system at micro air vehicle, need to introduce the attitude information that error does not increase in time, come restraining inertial navigation system attitude error to disperse.The inhibition that the error of miniature inertial navigation system is dispersed is minitype inertial integrated navigation system key in application in micro air vehicle, the method of proposition in this regard mainly contains and utilizes accelerometer to adopt the mode of weighting as obliquity sensor both at home and abroad, merge with the attitude of little inertial navigation system as: in August, 2004, the master thesis of university is raised Birmingham by the U.S.: Design Of An Autopilot ForSmall Unmanned Aerial Vehicles, 75-78; The inventor herein proposes to utilize modern optimal State Estimation method in 2006 the 6th phases applied science journal paper realization of the MEMS-INS minute vehicle attitude and heading reference system " research "---and Kalman filtering realizes that micro-inertial navigation system and accelerometer obtain the method that horizontal attitude merges etc.

The accelerometer obliquity sensor is influenced by carrier dynamically, the conventional thinking of existing document is that the use to the accelerometer obliquity sensor retrains, information according to Inertial Measurement Unit IMU and boat appearance system is judged the dynamic process of carrier, after the carrier dynamic process surpasses certain limit, no longer utilize the accelerometer obliquity sensor to carry out attitude measurement, because the micro air vehicle moment of inertia is little, maneuverability, be subjected to the influence of air-flow serious, attitude range and rate of change are much larger than general unmanned plane, and this mode is not suitable for micro air vehicle.

Three, summary of the invention

Fundamental purpose of the present invention is, improve in the existing document deficiency to accelerometer obliquity sensor error processing method under the dynamic condition, under Navigation, Guidance and Control (GNC) closed loop condition, from the GNC loop, extract key feature information, exploration is adapted to the new way of micro air vehicle characteristics, suppresses the attitude error that is dynamically caused by the accelerometer obliquity sensor in-flight of micro air vehicle.

Content of the present invention is that the little inertia combined navigation system of the MEMS of micro air vehicle utilizes guidance information in the micro aircraft GNC closed loop at its place to realize that attitude determines method, and its characteristics realize grinding by following steps:

(8) guidance algorithm of micro aircraft GNC closed loop resolves step: the position and the position of expecting way point current according to micro air vehicle, calculate the heading of expectation, the heading and the current flight direction of expectation are subtracted each other, the difference of gained multiply by a coefficient k, the micro air vehicle roll angle that obtains expecting, wherein the desirable scope of coefficient k is 0.2 to 0.5;

(9) MEMS Inertial Measurement Unit signals collecting step: gather the output signal of MEMS Inertial Measurement Unit, obtain the angular velocity and the specific force of micro air vehicle;

(10) the inertial navigation algorithm resolves step: the angular velocity that step (2) is collected and than force signal by the flow process of strap inertial navigation algorithm, calculates the navigation information of attitude, speed and the position of micro air vehicle;

(11) obtain the metrical information step of position and attitude: the position, speed and the course information that read GPS, utilize the ratio force signal that constantly collects recently simultaneously, the measuring principle of pressing the accelerometer obliquity sensor is calculated the roll angle and the angle of pitch of micro air vehicle;

(12) the self-adaptation set-up procedure of Kalman filter observing matrix: the roll angle of the expectation that obtains with step (1) is an independent variable, by a piecewise function, the variance of horizontal attitude in the observation noise matrix of real-time computer card Thalmann filter, keep multiple relation in real time in the observation noise matrix of the Kalman filter of calculating between the variance of horizontal attitude and its initial value, this multiple is the dependent variable of piecewise function, and the concrete funtcional relationship of this piecewise function is determined by the flight test of reality;

(13) combined filter and correction step: utilize step (5) to adjust the Kalman filter of observing matrix parameter in real time, the metrical information that step (4) is obtained is handled, the error of attitude, speed and position that estimating step (2) inertial navigation calculates, in attitude, speed and position that step (2) obtains, deduct the error that this step estimates, and attitude, speed and the positional information of replacement step (2);

(14) feedback step of attitude, positional information: the attitude information that step (6) obtains is input to the control module of micro air vehicle, and control module changes by the attitude of the rudder face control micro air vehicle of micro air vehicle; Position and course information that step (6) obtains are input to the system guide module, are used for completing steps (1).

The present invention starts with in the angle of control closed-loop system from navigating, guiding, based on Kalman filtering the average essence of reinforcement is arranged most, in conjunction with differentiation to the state of flight of micro air vehicle, the method of the observation noise matrix by the little inertia combined navigation system Kalman filter of dynamic adjustment, improve the precision of little inertia combined navigation system attitude, weaken the dynamic obliquity sensor in-flight of micro air vehicle to the correction of system's attitude, weaken the dynamically attitude correction in Navigation, Guidance and Control loop equivalence interference in-flight, improve the flight quality of micro air vehicle.

Four, Figure of description

Fig. 1 is minute vehicle Navigation, Guidance and Control system principle diagram.

Fig. 2 is a miniature Navigation, Guidance and Control of the present invention system closed loop block diagram.

Fig. 3 is the little inertia combined navigation system composition frame chart of MEMS among Fig. 2.

The roll angle curve synoptic diagram of micro air vehicle autonomous flight in Fig. 4 prior art.

Fig. 5 is the roll angle curve synoptic diagram of micro air vehicle autonomous flight of the present invention.

Five, embodiment

Principle of the present invention is:

In traditional GNC closed loop, navigational system provides navigational parameter to feed back to guidance and flight control algorithm, shown in the theory diagram that each module and solid line among Fig. 1 constitute.The present invention is then on the basis in traditional GNC loop, the feedforward control of increase from the system guide module to navigation subsystem, shown in the dotted line of Fig. 1, utilize guidance information, control combination Kalman filter noise matrix, realize of the self-adaptation adjustment of Kalman filter observation noise battle array with state of flight, by parameter adjustment, improve the adaptivity of little inertia combined navigation system to state of flight, realize that the attitude of existing mini-aircraft inertial integrated navigation system under dynamic flying condition determine, improve the attitude accuracy under the dynamic flying condition and the stability of flight.

Determine for the attitude that realizes mini-aircraft inertial integrated navigation system, adapt to dynamic flying condition, need finish the work:

(1) guidance algorithm of micro aircraft GNC closed loop resolves

Micro air vehicle calculates the course angle ψ of expectation according to the position of current position and expectation way point _e, the course angle ψ that the current actual measurement of expecting of course angle and micro navigation system is obtained subtracts each other, and the difference of gained multiply by a coefficient k, and the micro air vehicle roll angle that obtains expecting, k are scale-up factor, and span is 0.2 to 0.5.

With the micro air vehicle takeoff point is initial point, with the north orientation is x axle positive dirction, with the east orientation is y axle positive dirction, sets up local the earth horizontal coordinates, under this coordinate system, course angle is along x axle positive dirction to be 0 degree, turn to then course angle increase of right side, the scope of course angle is 0～360 degree, and the real time position coordinate of micro air vehicle is X, Y, wherein X is that horizontal ordinate, Y are ordinate, and the way point coordinate that current expectation is flown to is P _x, P _y, P wherein _xBe horizontal ordinate, P _yBe ordinate, then Qi Wang heading ψ _eBe calculated as follows:

Then Qi Wang heading is calculated as follows:

As Px＞X, Py 〉=Y, ${\mathrm{\ψ}}_e={\mathrm{tg}}^{-1}\frac{\mathrm{Py}-Y}{\mathrm{Px}-X};$

Work as Px=X, Py＞Y, ψ _e=pi/2;

As Px＜X, ${\mathrm{\ψ}}_e={\mathrm{tg}}^{-1}\frac{\mathrm{Py}-Y}{\mathrm{Px}-X}+\mathrm{\π};$

Work as Px=X, Py＜Y, ψ _e=3 pi/2s;

As Px＞X, Py＜Y ${\mathrm{\ψ}}_e={\mathrm{tg}}^{-1}\frac{\mathrm{Py}-Y}{\mathrm{Px}-X}+2\mathrm{\π};$

Guidance algorithm is pressed the roll angle γ that following formula generates expectation _e: γ _e=k (ψ _e-ψ),

Wherein ψ is the course angle that little inertia combined navigation system actual measurement obtains.

(2) MEMS Inertial Measurement Unit signals collecting step: gather the output signal of MEMS Inertial Measurement Unit, obtain the angular velocity and the specific force of micro air vehicle;

(3) the inertial navigation algorithm resolves

The angular velocity that utilization collects and than force signal by the flow process of strap inertial navigation algorithm, calculates the navigation informations such as attitude, speed and position of micro air vehicle.The initial attitude that inertial navigation resolves, speed and position are imported by the outside.

Body axis system is followed successively by angular velocity omega around the roll axle with respect to the component at three axles of body system of the angular velocity of local geographic coordinate system _x, around the angular velocity omega of pitch axis _y, around the angular velocity omega of azimuth axis _z, the computation period of inertial navigation is Δ t, three components of the angle delta θ that body turns in the Δ t time are followed successively by roll shaft angle increment Delta θ successively _x, pitch axis angle increment Δ θ _y, azimuth axis angle increment Δ θ _z, the angle that then turns over is expressed as with matrix-style:

$\mathrm{\&Delta;\&theta;}=\left[\begin{array}{ccc}0& {-\mathrm{\&Delta;\&theta;}}_z& {\mathrm{\&Delta;\&theta;}}_y\\ {\mathrm{\&Delta;\&theta;}}_z& 0& {-\mathrm{\&Delta;\&theta;}}_x\\ -{\mathrm{\&Delta;\&theta;}}_y& {\mathrm{\&Delta;\&theta;}}_x& 0\end{array}\right]=\left[\begin{array}{ccc}0& {-\mathrm{\&omega;}}_z& {\mathrm{\&omega;}}_y\\ {\mathrm{\&omega;}}_z& 0& {-\mathrm{\&omega;}}_x\\ {-\mathrm{\&omega;}}_y& {\mathrm{\&omega;}}_{\mathrm{<figure-callout\; id="0"\; label="x"\; filenames="H2007100214015E00031.png"\; state="\{\{state\}\}">x</figure-callout>}}& 0\end{array}\right]\mathrm{\&Delta;t}$

The attitude matrix of t carrier constantly is

$C_n^b\left(t\right)=\left[\begin{array}{ccc}\cos \mathrm{\&gamma;}\cos \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\sin \mathrm{\&theta;}\sin \mathrm{\&psi;}& -\cos \mathrm{\&gamma;}\sin \mathrm{\&psi;}+\sin \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}& -\sin \mathrm{\&gamma;}\cos \mathrm{\&theta;}\\ \cos \mathrm{\&theta;}\sin \mathrm{\&psi;}& \cos \mathrm{\&theta;}\cos \mathrm{\&psi;}& \sin \mathrm{\&theta;}\\ \sin \mathrm{\&gamma;}\cos \mathrm{\&psi;}-\cos \mathrm{\&gamma;}\sin \mathrm{\&theta;}\sin \mathrm{\&psi;}& -\sin \mathrm{\&gamma;}\sin \mathrm{\&psi;}-\cos \mathrm{\&gamma;}\sin \mathrm{\&theta;}\cos \mathrm{\&psi;}& \cos \mathrm{\&gamma;}\cos \mathrm{\&theta;}\end{array}\right]$

So t+ Δ t constantly attitude matrix by Try to achieve from t attitude battle array recursion constantly, with C _n ^b(t+ Δ t) is abbreviated as

Then the t+ Δ t moment, attitude informations such as the pitching angle theta of inertial navigation, roll angle γ and course angle ψ are obtained by following formula.

$\left.\begin{array}{c}\mathrm{\&theta;}={\sin }^{-1}\left(T_{23}\right)\\ \mathrm{\&gamma;}={\mathrm{tg}}^{-1}\left(\frac{T_{13}}{T_{33}}\right)\\ \mathrm{\&psi;}={\mathrm{tg}}^{-1}\left(\frac{T_{21}}{T_{32}}\right)\end{array}\right\}$

On the basis of attitude algorithm,, obtain the acceleration of motion a in the local geographic coordinate system according to current attitude and specific force _N, a _E, a _D, wherein, a _NBe the acceleration of motion of north orientation, a _EThe acceleration of motion of east orientation, a _DGround to acceleration of motion.The speed of micro air vehicle and position are obtained by the following formula recursion, V _N(t) speed of expression t moment micro air vehicle north orientation, V _E(t) speed of expression t moment micro air vehicle east orientation, V _D(t) expression t constantly micro air vehicle ground to speed, V _N(t+ Δ t), V _E(t+ Δ t), V _D(t+ Δ t) represent respectively t+ Δ t constantly north orientation, east orientation and ground to speed, R represents earth radius, with symbol latitude L _I, longitude λ _IWith height h _IRepresent the position of the micro air vehicle that obtained by the inertial navigation algorithm respectively, then the speed of inertial navigation and position can be calculated as follows.

$\left\{\begin{array}{c}{V}_N=(t+\mathrm{\&Delta;t})=V_N\left(t\right)+a_N\mathrm{\&Delta;t}\\ V_E(t+\mathrm{\&Delta;t})=V_E\left(t\right)+a_E\mathrm{\&Delta;t}\\ V_D(t+\mathrm{\&Delta;t})=V_D\left(t\right)+a_D\mathrm{\&Delta;t}\end{array}\right.$

$\left\{\begin{array}{c}{L}_I(t+\mathrm{\&Delta;t})=L_I\left(t\right)+\frac{V_N\left(t\right)\mathrm{\&Delta;t}}{R}\\ {\mathrm{\&lambda;}}_I(t+\mathrm{\&Delta;t})={\mathrm{\&lambda;}}_I\left(t\right)+\frac{V_E\left(t\right)\mathrm{\&Delta;t}}{R\cos \left(L_I\left(t\right)\right)}\\ h_I(t+\mathrm{\&Delta;t})=h_I\left(t\right)-V_D\left(t\right)\mathrm{\&Delta;t}\end{array}\right.$

(4) obtain the metrical information of position and attitude

Read position (the latitude L of GPS _G, longitude λ _GWith height h _G) and course angle ψ _G, utilize the ratio force signal that constantly collects recently simultaneously, the measuring principle of pressing the accelerometer obliquity sensor is calculated the roll angle γ of micro air vehicle _AWith pitching angle theta _A

(5) the self-adaptation set-up procedure of Kalman filter observing matrix

The roll angle γ of the expectation that obtains with step (1) _eBe independent variable, by a piecewise function, the variance R of horizontal attitude in the observation noise matrix of t+1 real-time computer card Thalmann filter constantly _T+1, keeping multiple relation in real time in the observation noise matrix of the Kalman filter of calculating between the variance of horizontal attitude and its initial value, this multiple relation has parabolic, and this multiple is the dependent variable of piecewise function, and piecewise function has following form:

The concrete funtcional relationship of piecewise function determines by the flight test of reality, wherein, and γ _E0The span of expression roll angle, R ₀Be the initial value of horizontal attitude variance, determine k according to the measurement noise size of accelerometer _RBe the public sector of the multiple relation of parabolic under the different conditions, obtain by test.

(6) combined filter and correction step

Utilize step (5) to adjust the Kalman filter of observing matrix parameter in real time, the metrical information that step (4) is obtained is handled, the error of attitude, speed and position that estimating step (2) inertial navigation calculates, in attitude, speed and position that step (2) obtains, deduct the error that this step estimates, and attitude, speed and the positional information of replacement step (2).

This Kalman filter is characterised in that:

The observation noise matrix of wave filter determines in real time that by step (5) state variable X comprises the north orientation platform error angle φ of strapdown inertial navitation system (SINS) _N, east orientation platform error angle φ _EWith ground to platform error angle φ _D, north orientation velocity error δ v _N, east orientation velocity error δ v _EWith ground to velocity error δ v _D, latitude error δ L, longitude error δ λ and height error δ h, totally 9, i.e. X=[φ _Nφ _Eφ _Dδ v _Nδ v _Eδ v _Dδ L δ λ δ h] ^T, wave filter is observed quantity with platform error angle and site error, observational variable is 6.

Handle as follows to the metrical information that step (4) is obtained:

If t+1 is the filtering moment, this moment is by GPS course angle ψ _G, the roll angle γ that obliquity sensor records _AAnd pitching angle theta _A, can obtain the attitude matrix C that attitude measurement information is determined _{N "} ^b

$C_{n^{\&prime;\&prime;}}^b=\left[\begin{array}{ccc}{\cos \mathrm{\&gamma;}}_A{\cos \mathrm{\&psi;}}_G+{\sin \mathrm{\&gamma;}}_A{\sin \mathrm{\&theta;}}_A{\sin \mathrm{\&psi;}}_G& -{\cos \mathrm{\&gamma;}}_A{\sin \mathrm{\&psi;}}_G+{\sin \mathrm{\&gamma;}}_A{\sin \mathrm{\&theta;}}_A{\cos \mathrm{\&psi;}}_G& -\sin {\mathrm{\&gamma;}}_A{\cos \mathrm{\&theta;}}_A\\ {\cos \mathrm{\&theta;}}_A{\sin \mathrm{\&psi;}}_G& {\cos \mathrm{\&theta;}}_A{\cos \mathrm{\&psi;}}_G& \sin {\mathrm{\&theta;}}_A\\ {\sin \mathrm{\&gamma;}}_A{\cos \mathrm{\&psi;}}_G-{\cos \mathrm{\&gamma;}}_A{\sin \mathrm{\&theta;}}_A{\sin \mathrm{\&psi;}}_G& {-\sin \mathrm{\&gamma;}}_A{\sin \mathrm{\&psi;}}_G-{\cos \mathrm{\&gamma;}\sin \mathrm{\&theta;}}_A{\cos \mathrm{\&psi;}}_G& {\cos \mathrm{\&gamma;}}_A{\cos \mathrm{\&theta;}}_A\end{array}\right]$

By attitude matrix C _{N "} ^bThree platform error angles of the mathematical platform of determining are designated as north orientation platform error angle φ _NA, east orientation platform error angle φ _EA, ground is to platform error angle φ _DG

Utilize this attitude matrix C of inertial navigation constantly _n ^b(t+1) transposition and C _{N "} ^bMultiply each other, obtain the mathematical platform error angle [φ of the attitude battle array correspondence of strapdown inertial navitation system (SINS) _Nφ _Eφ _D] ^TThe attitude and the corresponding mathematical platform error angle [φ of attitude battle array that measure with step (4) _NAφ _EAφ _DG] ^TBetween poor, promptly

$C_n^b{(t+1)}^T{C}_{n^{\&prime;\&prime;}}^b=\left[\begin{array}{ccc}1& ({\mathrm{\&phi;}}_D-{\mathrm{\&phi;}}_{\mathrm{DG}})& -({\mathrm{\&phi;}}_E-{\mathrm{\&phi;}}_{\mathrm{EA}})\\ -({\mathrm{\&phi;}}_D-{\mathrm{\&phi;}}_{\mathrm{DG}})& 1& ({\mathrm{\&phi;}}_N-{\mathrm{\&phi;}}_{\mathrm{NA}})\\ ({\mathrm{\&phi;}}_E-{\mathrm{\&phi;}}_{\mathrm{EA}})& -({\mathrm{\&phi;}}_N-{\mathrm{\&phi;}}_{\mathrm{NA}})& 1\end{array}\right]$

The platform error angle observed quantity of Kalman filtering is Inertial navigation resolves the position of the micro air vehicle that obtains and subtract each other the position of the micro air vehicle that GPS obtains, and obtains the observed quantity of position

T+1 total observed quantity Z of Kalman filter is constantly merged in observed quantity of platform error angle and position detection amount _T+1, then

Matrix is measured in definition

In the Kalman filter, state vector is 9 dimensions, and the variance battle array P of status predication is 9 * 9 matrix, initial valuation error variance battle array initial value P ₀, system noise variance battle array initial value Q ₀Observation noise variance battle array initial value R ₀The equal matrix initial value is all by the outside directly input of system, state transitions φ _{T+1, t}Be known matrix in the existing document, system noise matrix Q _tBy system noise variance matrix initial value Q ₀Determine observation noise matrix R _T+1Calculate by step (5).

Then the variance battle array of each filtering State Estimation constantly and the variance battle array of status predication can recursion obtain.Promptly at the filtering P of t constantly _tOn the basis,, obtain the filtering variance battle array P of the status predication of t+1 constantly by following Recursive Filtering equation _T+1|t, the filter gain matrix K _T+1, the state estimation value

And the variance battle array P of State Estimation _T+1Can calculate by following formula.

Q _t＝Q ₀

$P_{t+1|t}={\mathrm{\&phi;}}_{t+1,t}{P}_t{\mathrm{\&phi;}}_{t+1,t}^T+Q_t$

K _t+1＝P _t+1|tH _t+1 ^T(H _t+1P _t+1|tH _t+1 ^T+R _t+1) ^-1

${\hat{X}}_{t+1}=K_{t+1}{\mathrm{<figure-callout\; id="1"\; label="Z\; t\; +"\; filenames="H2007100214015E00012.png,H2007100214015E00021.png"\; state="\{\{state\}\}">Z</figure-callout>}}_{t+}$

P _t+1＝(I-K _t+1H _t+1)P _t+1|t

The state estimation value of aforementioned calculation

Be the error of the resultant attitude that obtains of step (4) inertial navigation, speed and position, in the resulting result of step (4), deduct estimated value

In the amount of correspondence, can improve the precision of the attitude of little inertia combined navigation system, reach the purpose of determining the micro air vehicle attitude.

(7) feedback step of attitude, positional information

The attitude information that step (6) obtains is input to the control module of micro air vehicle, and control module changes by the attitude of the rudder face control micro air vehicle of micro air vehicle; Position and course information that step (6) obtains are input to the system guide module, are used for completing steps (1).

Like this, the method of the observation noise matrix by the little inertia combined navigation system Kalman filter of dynamic adjustment, improve the precision of little inertia combined navigation system attitude, weaken the dynamic obliquity sensor in-flight of micro air vehicle to the correction of system's attitude, weaken the dynamically attitude correction in Navigation, Guidance and Control loop equivalence interference in-flight, improve the flight quality of micro air vehicle.

Among Fig. 1, solid arrow each module and that connect has been represented the basic logic connecting relation of Navigation, Guidance and Control closed loops, from the dotted arrow representative of the little inertia combined navigation system of sensing of drawing between micro air vehicle guidance algorithm module and the micro air vehicle flight control algorithm be expectation of the present invention roll angle transmission and utilize relation.

Fig. 2 is the further refinement to Fig. 1, micro air vehicle guidance algorithm module in position comparison module and the course control module pie graph 1, expectation pitching module, pitch control subsystem module and roll control module are formed the micro air vehicle flight control algoritic module among Fig. 1 jointly.Identical among dotted arrow and Fig. 1, shown the source and the transmission of information more clearly.

Fig. 3 is the further refinement of the little inertia combined navigation system of MEMS among Fig. 2, has shown the roll angle γ of the expectation of dotted line representative among Fig. 2 _eTo little inertia combined navigation system Kalman filter observation noise matrix R _T+1The control adjustment.

Fig. 4 is the roll angle data and curves when adopting minute vehicle left side orbit under the prior art situation, transverse axis is represented the time, the longitudinal axis is represented roll angle, be in left in 315 seconds to 330 seconds always and spiral, the fluctuation range of roll angle-2 is spent-32 degree, the transversal wave movement scope is big in-flight, has reached 30 degree.

Fig. 5 is after using the present invention, minute vehicle repeats left and right data and curves of spiraling, the flight course of curve reflection: after 80 seconds, 5 times spiral in a left side, and 4 times spiral in the right side, left and right spiraling mutually alternately, after using the present invention, in the orbit of a left side, the fluctuation range of roll angle is spent-30 degree about-18, and the fluctuation range 18 of roll angle is spent to 30 degree during right orbit.After using the present invention, fluctuation range obviously reduces, about 12 degree, and error has dwindled 60%.

Six, the effect of invention

The present invention starts with from the angle of Navigation, Guidance and Control closed-loop system, based on the optimum average weighted essence of Kalman filtering, differentiate in conjunction with the state of flight to micro air vehicle, the method of observation noise R by dynamic adjustment minitype combined navigation system Kalman filter, improve the precision of little inertia combined navigation system attitude, weaken in the micro air vehicle dynamic flying obliquity sensor to the observation correction of system's attitude, weaken the attitude correction equivalence in GNC loop and disturb, improve flight quality.

The orbit test effect:

For the little inertia combined navigation system in the micro air vehicle autonomous flight, the micro air vehicle turning flight that spirals is the worst state of flight that will often experience. Adopt in the orbit test of the present invention front and back, two groups of roll angle correlation curves such as Fig. 4 and Fig. 5, from curve, can find out, before employing the present invention, in the orbit, the attitude of micro air vehicle is round excursion 30 degree of desired value, after adopting the present invention, the attitude in the micro air vehicle autonomous flight is spent less than 12 round the fluctuation range of desired value, and fluctuation obviously reduces, the curve of flight and the deviation between the desired value obviously reduce, and error has dwindled 60%.

The present invention is directed to the measure error that obliquity sensor is introduced in the dynamic flying, utilize the method for dynamically adjusting the Kalman filter observation noise, improve the performance of little inertia combined navigation system, reduce measure error, improve the degree of having a smooth flight of micro air vehicle.

A large amount of Flight Test results show: the present invention is applied to minute vehicle, consist of the Navigation, Guidance and Control closed-loop system, minute vehicle can be realized autonomous attitude stabilization under the fitful wind environment of 5-6 meter per second and the navigation flight of way point, and the autonomous flight performance of micro air vehicle has reached the level of expectation. The present invention has very strong engineering using value.

Claims (3)

1. the attitude of a mini-aircraft inertial integrated navigation system is determined method, and its characteristics are to realize by following steps:

(1) guidance algorithm of micro aircraft GNC closed loop resolves step: the position and the position of expecting way point current according to micro air vehicle, calculate the heading of expectation, the heading and the current flight direction of expectation are subtracted each other, the difference of gained multiply by a coefficient k, the micro air vehicle roll angle that obtains expecting, wherein the desirable scope of coefficient k is 0.2 to 0.5;

(2) MEMS Inertial Measurement Unit signals collecting step: gather the output signal of MEMS Inertial Measurement Unit, obtain the angular velocity and the specific force of micro air vehicle;

(3) the inertial navigation algorithm resolves step: the angular velocity that step (2) is collected and than force signal by the flow process of strap inertial navigation algorithm, calculates the navigation information of attitude, speed and the position of micro air vehicle;

(4) obtain the metrical information step of position and attitude: the position, speed and the course information that read GPS, utilize the ratio force signal that constantly collects recently simultaneously, the measuring principle of pressing the accelerometer obliquity sensor is calculated the roll angle and the angle of pitch of micro air vehicle;

(5) the self-adaptation set-up procedure of Kalman filter observing matrix: the roll angle of the expectation that obtains with step (1) is an independent variable, by a piecewise function, the variance of horizontal attitude in the observation noise matrix of real-time computer card Thalmann filter, keep multiple relation in real time in the observation noise matrix of the Kalman filter of calculating between the variance of horizontal attitude and its initial value, this multiple is the dependent variable of piecewise function, and the concrete funtcional relationship of this piecewise function is determined by the flight test of reality;

(6) combined filter and correction step: utilize step (5) to adjust the Kalman filter of observing matrix parameter in real time, the metrical information that step (4) is obtained is handled, the error of attitude, speed and position that estimating step (2) inertial navigation calculates, in attitude, speed and position that step (2) obtains, deduct the error that this step estimates, and attitude, speed and the positional information of replacement step (2);

(7) feedback step of attitude, positional information: the attitude information that step (6) obtains is input to the control module of micro air vehicle, and control module changes by the attitude of the rudder face control micro air vehicle of micro air vehicle; Position and course information that step (6) obtains are input to the system guide module, are used for completing steps (1).

2. the attitude of mini-aircraft inertial integrated navigation system as claimed in claim 1 is determined method, it is characterized in that the micro air vehicle roll angle γ that expects _eComputing method be:

With the micro air vehicle takeoff point is initial point, with the north orientation is x axle positive dirction, with the east orientation is y axle positive dirction, sets up local the earth horizontal coordinates, under this coordinate system, course angle is along x axle positive dirction to be 0 degree, turn to then course angle increase of right side, the scope of course angle is 0～360 degree, and the real time position coordinate of micro air vehicle is X, Y, wherein X is that horizontal ordinate, Y are ordinate, and the way point coordinate that current expectation is flown to is P _x, P _y, P wherein _xBe horizontal ordinate, P _yBe ordinate, then Qi Wang heading ψ _eBe calculated as follows:

As Px＞X, Py 〉=Y,

Work as Px=X, Py＞Y, ψ _e=pi/2;

As Px＜X,

Work as Px=X, Py＜Y, ψ _e=3 pi/2s;

As Px＞X, Py＜Y

Guidance algorithm is pressed the roll angle γ that following formula generates expectation _e: γ _e=k (ψ _e-ψ),

Wherein ψ is the course angle that little inertia combined navigation system actual measurement obtains, and k is a scale-up factor, and span is 0.2 to 0.5.

3. the attitude of mini-aircraft inertial integrated navigation system as claimed in claim 1 is determined method, it is characterized in that Kalman filtering observing matrix self-adapting regulation method is:

The roll angle γ of the expectation that obtains with step (1) _eBe independent variable, by a piecewise function, the variance R of horizontal attitude in the observation noise matrix of t+1 real-time computer card Thalmann filter constantly _I+1, keeping multiple relation in real time in the observation noise matrix of the Kalman filter of calculating between the variance of horizontal attitude and its initial value, this multiple relation has parabolic, and this multiple is the dependent variable of piecewise function, and piecewise function has following form:

The concrete funtcional relationship of piecewise function determines by the flight test of reality, wherein, and γ _E0The span of expression roll angle, R ₀Be the initial value of horizontal attitude variance, determine k according to the measurement noise size of accelerometer _RBe the public sector of the multiple relation of parabolic under the different conditions, obtain by test.

Images