CN106342284B - A kind of flight carrier attitude is determined method - Google Patents

A kind of flight carrier attitude is determined method

Info

Publication number
CN106342284B
CN106342284B CN200810076417.0A CN200810076417A CN106342284B CN 106342284 B CN106342284 B CN 106342284B CN 200810076417 A CN200810076417 A CN 200810076417A CN 106342284 B CN106342284 B CN 106342284B
Authority
CN
China
Prior art keywords
attitude
angle
flight carrier
theta
utilize
Prior art date
Application number
CN200810076417.0A
Other languages
Chinese (zh)
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 CN200810076417.0A priority Critical patent/CN106342284B/en
Application granted granted Critical
Publication of CN106342284B publication Critical patent/CN106342284B/en

Links

Abstract

The invention discloses a kind of flight carrier attitude and determine method, belong to attitude measurement field. Key step comprises: coordinate system and the attitude matrix of setting up flight carrier; Pick-up transducers signal; Utilize gyro to measure value to carry out strapdown attitude algorithm, obtain attitude quaternion and the attitude angle of flight carrier; Utilize magnetometer survey value to resolve flight carrier course angle; Utilize velocity information to compensate acceleration measurement, solve the angle of pitch and roll angle; Solve by magnetic strength and take into account the definite attitude quaternion of accelerometer information; Design Kalman filter, estimated state vector; Proofread and correct attitude quaternion and extract attitude angle. This method, by gyro, accelerometer, magnetometer, velocity sensor combination, meets the requirement of flight carrier real-time attitude control; Gyroscopic drift is incorporated in system mode vector, it is estimated to proofread and correct in real time, adopt magnetometer survey value directly to calculate course angle simultaneously, avoid the calculating of geomagnetic fieldvector under geographic coordinate system, improved the definite precision of attitude.

Description

A kind of flight carrier attitude is determined method
Technical field
The present invention relates to a kind of flight carrier attitude and determine method, belong to attitude measurement field.
Background technology
It is the prerequisite of attitude control that attitude is determined, significant for aircraft, guided missile, attitude control of satellite fixture. Attitude is determinedBe by sensor measurement information, through data processing, obtain the attitude of flight carrier with respect to space coordinate system. Utilize gyroCarry out strapdown attitude with accelerometer measures information and determine, there is the features such as independence, antijamming capability are strong, but precision is mainly gotCertainly in the precision of inertia device, and gyroscopic drift will cause attitude to determine that error accumulates in time, can not meet long-time high accuracyThe application requirements that attitude is definite. In order to address this problem, people, by gyro, accelerometer and magnetometer combination, utilize gravityWith geomagnetic fieldvector as a reference, composite design Kalman filter, proofreaies and correct the attitude resolved by gyro to measure information, has suppressedThe attitude that gyroscopic drift causes is determined the accumulation of error, but, when flight carrier is under large motor-driven environment, accelerometer measuresThe specific force acceleration and the disturbing acceleration that have comprised gravitational vectors and flight carrier itself, rely on merely accelerometer be difficult to from thanIn power, required gravitational vectors is separated, i.e. inertia force and gravitation undistinguishable principle, this will cause attitude to determine that error is fastSpeed increases. For this reason, people set algorithms of different or adjust online Kalman filtering noise side according to flight carrier different motion statePoor battle array, increases under large motor-driven environment the dependency degree to utilizing the attitude information that gyro to measure resolves, reduce to measurement information canReliability, this performance to gyro has proposed higher requirement. Application number is that " microminiature is dynamically carried for 200510011763.7 patentBody attitude measuring and measuring method thereof " a kind of microminiature carrier attitude measuring apparatus and method, this measuring method proposedKey is to have improved algorithm design, determines that in algorithm design, state vector is taken as attitude matrix element, based on attitude matrix in attitudeThe differential equation has been set up Kalman filtering state equation, and Kalman filtering measuring value is the measured value of magnetometer and accelerometer. ByIn having introduced velocity sensor information, the impact that the acceleration that can eliminate motion carrier itself is measured gravitational vectors, has improved fortuneMobile carrier is the definite precision of attitude under large motor-driven environment. Although the method can solve motion carrier acceleration gravitational vectors is surveyedThe problem that affects of amount, but also exist some significantly not enough: its Measurement Algorithm is not estimated gyroscopic drift as system modeProofread and correct, reduced gyro to measure precision and attitude determination accuracy; Geomagnetic fieldvector adopts the fixed die of simplifying under inertial coodinate systemType, causes the deviation between its measured value and estimated value larger, thereby has reduced attitude determination accuracy.
Summary of the invention
In order to overcome the above-mentioned defect of prior art, the present invention proposes a kind of flight carrier attitude and determine method, realize flight and carryBody attitude is determined and gyroscopic drift is estimated to proofread and correct in real time, improves attitude determination accuracy, and the concrete steps that realize the method are:
Step 1: coordinate system and the attitude matrix of setting up flight carrier. Consult Fig. 1, X in body axis system b systemb,Yb,ZbThreeIndividual normal axis respectively along the transverse axis of flight carrier to the right, before Y, vertical pivot upwards. Geographic coordinate system n system, Xn,Yn,ZnThreeIndividual normal axis points to respectively east, north, sky. Attitude angle is defined as: course angle ψ is (180 °, 180 °), and pitching angle theta is (90 °, 90 °),Roll angle γ is (180 °, 180 °). According to the rotation order of course-pitching-roll, obtain the appearance of the attitude angle form of flight carrierState matrixFor:
C n b = c γ c ψ + s γ s θ s ψ - c γ s ψ + s γ s θ c ψ - s γ c θ c θ s ψ c θ c ψ s θ s γ c ψ - c γ s θ s ψ - s γ s ψ - c γ s θ c ψ c γ c θ - - - ( 1 )
Wherein s and c are respectively writing a Chinese character in simplified form of function sin and cos, definition flight carrier attitude quaternion Q=[q0 q1 q2 q3]T,Obtain the attitude matrix of hypercomplex number formFor:
C n b = q 0 2 + q 1 2 - q 2 2 - q 3 2 2 ( q 1 q 2 + q 0 q 3 ) 2 ( q 1 q 3 - q 0 q 2 ) 2 ( q 1 q 2 - q 0 q 3 ) q 0 2 - q 1 2 + q 2 2 - q 3 2 2 ( q 2 q 3 + q 0 q 1 ) 2 ( q 1 q 3 + q 0 q 2 ) 2 ( q 2 q 3 - q 0 q 1 ) q 0 2 - q 1 2 - q 2 2 + q 3 2 - - - ( 2 )
Step 2: pick-up transducers signal. Utilize gyro, accelerometer, individual axis velocity sensor to measure respectively flight carrierAngular speed, acceleration and speed, be converted to angular velocity omega through A/Db=[ωbx ωby ωbz]TAnd acceleration fb=[fbx fby fbz]TAnd speed vy, utilize magnetometer to obtain geomagnetic fieldvector mb=[mbx mby mbz]T
Step 3: utilize angular velocity omegab, adopt strapdown attitude derivation algorithm, ask for and utilize angular velocity omegabThe flight carrier appearance of resolvingState hypercomplex number QIAnd pitching angle thetaIAnd roll angle γI
Step 4: utilize geomagnetic fieldvector mbSolve the course angle ψ of flight carrierS. The pitching angle theta of utilizing step 3 to obtainIWithRoll angle γISet up the transition matrix between body axis system b system and horizontal coordinates h system, by geomagnetic fieldvector mbBe transformed into waterIn flat coordinate system h system, calculate the magnetic heading angle of flight carrier, through magnetic declination, correction obtains course angle ψS
Step 5: utilize speed vyTo acceleration fbCompensation, obtains the projection value of gravitational vectors in body axis system b system, profitAt the transformational relation between geographic coordinate system n system and body axis system b system, calculate the angle of pitch of flight carrier with gravitational vectors θSWith roll angle γS
When flight carrier is under state static or that acceleration is very little, ignore the impact of acceleration, acceleration fbBe similar to and thinkThe projection value of gravitational vectors in body axis system b system, when flight carrier is under large motion of automobile environment, as speed change degree fliesUnder row and turn condition, acceleration fbTo no longer reflect real gravitational vectors, therefore, to acceleration fbUtilize speed vyCompensationAs follows:
f b x ′ = f b x + ω ^ b z · v y f b y ′ = f b y - v · y - - - ( 3 )
Wherein f 'bxAnd f 'byBeing respectively the linear acceleration and the centripetal acceleration that cause through the variable motion by flight carrier and rotation mendsAccekeration after repaying,For along ZbThe angular speed of the gyro of axle after gyroscopic drift compensation, vyFor along flight carrier bodyCoordinate system YbThe individual axis velocity measurement value sensor that axle is placed, asks for roll angle γSAnd pitching angle thetaSFor:
γ S = a tan ( - f b x ′ / f b z ) θ S = a tan ( ( - f b y ′ / f b z ) sinγ S ) - - - ( 4 )
Step 6: for course angle ψS, pitching angle thetaSAnd roll angle γS, utilize the relation between attitude quaternion and attitude angle,Ask for by geomagnetic fieldvector mbWith acceleration fbThe flight carrier attitude quaternion Q resolvingS
Step 7: adopt Kalman Filter Technology, design Kalman filter, to utilizing angular velocity omegabThe flight carrier appearance of resolvingState hypercomplex number QI, utilize geomagnetic fieldvector mbWith acceleration fbThe flight carrier attitude quaternion Q resolvingS, gyro to measure value ωb,Carry out the data processing of Kalman filtering, estimate and utilize angular velocity omegabThe flight carrier attitude quaternion Q resolvingICorresponding attitudeError quaternionWith gyroscopic drift errorComprise following sub-step:
1. set up system Kalman filtering state equation
Gyro to measure model is established as: ωb=ω+b+wg, wherein ωbFor gyro output valve, ω is true angular speed, wgForGyroscopic drift white noise, b is gyroscopic drift, meets equationwbFor gyroscopic drift random walk white noise
Definition utilizes angular velocity omegabThe flight carrier attitude quaternion Q resolvingICorresponding attitude error hypercomplex number isWhereinReal flight carrier attitude quaternion is Q, and Q can be expressed as:
Q = Q I ⊗ Q I e - - - ( 5 )
Differentiated in (5) formula both sides:
Q · = Q · I ⊗ Q I e + Q I ⊗ Q · I e - - - ( 6 )
Q · I e = 1 2 Q I e ⊗ ω b - 1 2 ω ^ b ⊗ Q I e - - - ( 7 )
:
q → · e = - [ ω ^ b × ] · q → e - 1 2 Δ b - 1 2 w g - - - ( 8 )
WhereinFor gyroscopic drift evaluated error, b is that gyro truly drifts about,For gyroscopic drift estimated value,ForSkew symmetric matrix,For angular velocity omegabValue after gyroscopic drift compensation, sets up system Kalman filtering state equationFor:
X · ( t ) = F ( t ) · X ( t ) + G ( t ) · W ( t ) - - - ( 9 )
Wherein X (t) is the system mode vector in t moment, and F (t) and G (t) are t moment system mode transfer matrix and noise matrix,W (t) is system noise vector.
System mode vector: X (t)=[qe1 qe2 qe3 Δbx Δby Δbz]T
System noise vector: W (t)=[wgx wgy wgz wbx wby wbz]T
Wherein qe1、qe2、qe3For attitude error hypercomplex number QIeThree components, Δ bx、Δby、ΔbzFor gyroscopic drift is estimatedError, wgx、wgy、wgzFor gyroscopic drift white noise, wbx、wby、wbzFor gyroscopic drift random walk white noise, systemState-transition matrix F (t) and noise matrix G (t) are:
F ( t ) = - [ ω ^ b × ] - 1 2 I 3 × 3 0 3 × 3 0 3 × 3 , G ( t ) = - 1 2 I 3 × 3 0 3 × 3 0 3 × 3 I 3 × 3
In formula, I representation unit matrix, 0 represents full null matrix, the dimension of subscript representing matrix.
2. set up system Kalman filtering measurement equation
Definition utilizes geomagnetic fieldvector mbWith acceleration fbThe flight carrier attitude quaternion Q resolvingSCorresponding attitude error quaternaryNumber is QSe=[1 qse1 qse2 qse3]T:
Q = Q I ⊗ Q I e Q = Q S ⊗ Q S e - - - ( 10 )
Definition vectorObtained by formula (10):
Y = Q S - 1 ⊗ Q I = Q S e ⊗ Q I e - 1 = 1 - q s e 1 - q s e 2 - q s e 3 q s e 1 1 - q s e 3 q s e 2 q s e 2 q s e 3 1 - q s e 1 q s e 3 - q s e 2 q s e 1 1 · 1 - q e 1 - q e 2 - q e 3 - - - ( 11 )
Ignore second order term, the system Kalman filtering measurement equation of obtaining is:
Z ( t ) = Y ( 2 ) Y ( 3 ) Y ( 4 ) = q s e 1 - q e 1 q s e 2 - q e 2 q s e 3 - q e 3 = H ( t ) X ( t ) + V ( t ) - - - ( 12 )
Wherein measurement matrix H (t)=[I3×3 03×3], V (t) is measurement noise, is approximately white noise.
3. system Kalman filter equation discretization and state estimation
1. the system Kalman filter equation of and 2. setting up is carried out to discretization, obtain discretization Kalman filtering state equation and amountSurvey equation is:
X k = Φ k , k - 1 X k - 1 + Γ k - 1 W k - 1 Z k = H k X k + V k - - - ( 13 )
A wherein step transition matrixSystem noise drives battle arrayT is the discretization cycle, system noise variance battle array QkWith measuring noise square difference battle array RkFor:
Q k = d i a g σ g x 2 σ g y 2 σ g z 2 σ b x 2 σ b x 2 σ b x 2 - - - ( 14 )
Rk=diag[δqe1 2 δqe2 2 δqe3 2] (15)
Wherein σgx,σgy,σgzFor gyroscopic drift white noise root mean square, σbx,σby,σbzFor all sides of gyroscopic drift random walk white noiseRoot, δ qe1,δqe2,δqe3For flight carrier attitude quaternion evaluated error, to utilizing angular velocity omegabThe flight carrier attitude four of resolvingThe number Q of unitIWith geomagnetic fieldvector mbAnd acceleration fbThe flight carrier attitude quaternion Q resolvingS, by (11) formula computer card GermaniaFiltering measuring value Zk, adopt discrete type Kalman filtering fundamental equation to carry out filtering estimation, obtain utilizing angular velocity omegabThat resolves fliesRow attitude of carrier hypercomplex number QICorresponding attitude error hypercomplex number QIeEstimated valueAnd gyroscopic drift evaluated error
Step 8: proofread and correct and utilize angular velocity omegabThe flight carrier attitude quaternion Q resolvingIAnd extract flight carrier attitude angle, navigateTo angle ψ, pitching angle theta and roll angle γ. Utilize the attitude error hypercomplex number estimating in step 7Correction utilizes angular velocity omegabThe flight carrier attitude quaternion Q resolvingI, obtain current time flight carrier attitude quaternion optimal estimation value
Q ^ = Q I ⊗ Q ^ I e - - - ( 16 )
Utilization obtainsObtain corresponding attitude matrix, utilize attitude matrix to extract flight carrier attitude angle, course angle ψ,Pitching angle theta and roll angle γ. To the gyroscopic drift evaluated error obtaining through Kalman filteringObtain gyroscopic drift estimated valueAdoptProofread and correct gyro to measure value, thereby improve gyro to measure precision. WillAs next moment attitude algorithm hypercomplex numberInitial value, returns to step 2 pick-up transducers signal, carries out the attitude algorithm in next moment, cycle calculations.
The invention has the beneficial effects as follows: 1) by gyro, accelerometer, micro-magnetometer, velocity sensor combination, thereby metThe requirement of flight carrier real-time attitude control. 2) gyroscopic drift is incorporated in system mode vector, it is estimated in real timeProofread and correct, thereby improved gyro to measure precision and attitude determination accuracy. 3) adopt magnetometer survey value directly to calculate course angle, keep awayExempt from the calculating of geomagnetic fieldvector under geographic coordinate system, improved the definite precision of attitude.
Brief description of the drawings
Fig. 1 is body axis system b system and the geographic coordinate system n system of flight carrier
Fig. 2 is that flight carrier attitude of the present invention is determined schematic diagram
Fig. 3 is that flight carrier attitude of the present invention is determined method realization flow figure
Detailed description of the invention
The present embodiment adopts three axle rate gyroscopes, three axis accelerometer, three axis magnetometer, the single shaft based on micro-electromechanical technologyVelocity sensor is as system sensor, and pick-up transducers signal, carries out flight carrier attitude and determine, with reference to Fig. 3 flight carrierAttitude is determined method realization flow, and the attitude algorithm that specifically completes the flight carrier of current time need to complete following steps:
Step 1: coordinate system and the attitude matrix of setting up flight carrier. Consult Fig. 1, X in body axis system b systemb,Yb,ZbThreeIndividual normal axis respectively along the transverse axis of flight carrier to the right, before Y, vertical pivot upwards. X in geographic coordinate system n systemn,Yn,ZnThreeIndividual normal axis points to respectively east, north, sky. Three axle rate gyroscopes, three axis accelerometer, three axis magnetometer are respectively along flight carrierBody axis system three axles are placed, and individual axis velocity sensor is along body axis system YbAxle is placed, and attitude angle is defined as: course angle ψ is(180 °, 180 °), pitching angle theta is (90 °, 90 °), roll angle γ is (180 °, 180 °), the attitude matrix of attitude angle formFor:
C n b = c γ c ψ + s γ s θ s ψ - c γ s ψ + s γ s θ c ψ - s γ c θ c θ s ψ c θ c ψ s θ s γ c ψ - c γ s θ s ψ - s γ s ψ - c γ s θ c ψ c γ c θ - - - ( 1 )
Definition flight carrier attitude quaternion Q=[q0 q1 q2 q3]T, the attitude matrix of hypercomplex number formFor:
C n b = q 0 2 + q 1 2 - q 2 2 - q 3 2 2 ( q 1 q 2 + q 0 q 3 ) 2 ( q 1 q 3 - q 0 q 2 ) 2 ( q 1 q 2 - q 0 q 3 ) q 0 2 - q 1 2 + q 2 2 - q 3 2 2 ( q 2 q 3 + q 0 q 1 ) 2 ( q 1 q 3 + q 0 q 2 ) 2 ( q 2 q 3 - q 0 q 1 ) q 0 2 - q 1 2 - q 2 2 + q 3 2 - - - ( 2 )
Step 2: pick-up transducers signal. Setting sensor sample rate is 0.05s, utilizes three axle rate gyroscopes, three axles to accelerateDegree meter, individual axis velocity sensor are measured respectively angular speed, acceleration and the velocity information of flight carrier, are converted to through A/DCurrent time angular velocity omegabx=0.01°/s、ωby=0.02°/s、ωbz=0.03 °/s, acceleration fbx=0.08m/s2、 fby=-0.08m/s2、fbz=-9.76m/s2, speed vy=50m/s, utilizes three axis magnetometer to obtain geomagnetic fieldvector mbx=-80、mby=3500、mbz=-4500, wherein conversion coefficient is 15000/Gs.
Step 3: the angular velocity omega that utilizes three axle rate gyroscopes to measurebx,ωby,ωbz, adopt strapdown attitude derivation algorithm, ask for profitUse angular velocity omegabThe flight carrier attitude quaternion Q resolvingIAnd pitching angle thetaIAnd roll angle γI. If the attitude angle of initial time is equalBe 0.5 °, three axle rate gyroscope drift initial estimatesBe 0.001 °/s, angular velocity omegabx,ωby,ωbzThrough topAfter spiral shell drift compensation, be:
WhereinFor the angular speed after gyroscopic drift compensation, utilizeSolve by strapdown attitudeArrive flight carrier by angular velocity omegabx,ωby,ωbzThe flight carrier attitude quaternion Q resolvingIAnd pitching angle thetaIAnd roll angle γIFor:
θI=0.50046°,γI=0.50094°
Step 4: utilize geomagnetic fieldvector to solve the course angle ψ of flight carrierS. The pitching angle theta of utilizing step 3 to obtainIAnd rollAngle γISet up the transition matrix between body axis system b system and horizontal coordinates h system, by geomagnetic fieldvector mbx,mbyBe transformed into waterIn flat coordinate system h system:
{ m h x = cosγ I · m b x + sinγ I · m b z = - 119 m h y = sinγ I · sinθ I · m b x + cosθ I · m b y - cosγ I · sinθ I · m b z = 3539 - - - ( 4 )
Wherein mbx,mbyFor geomagnetic fieldvector mbx,mbyBe transformed into the value in horizontal coordinates h system, ask for magnetic heading angle ψmFor:
&psi; m = &pi; 2 , m h y = 0 , m h x < 0 - &pi; 2 , m h y = 0 , m h x > 0 - a t a n ( m h x / m h y ) , m h y > 0 &pi; - a tan ( m h x / m h y ) , m h y < 0 , m h x < 0 - &pi; - a tan ( m h x / m h y ) , m h y < 0 , m h x > 0 - - - ( 5 )
Through magnetic declination, correction obtains the flight carrier course angle ψ that utilizes geomagnetic fieldvector to solveS=-0.552°
Step 5: the speed v of utilizing individual axis velocity sensor to obtainyTo acceleration fbx,fbyCompensate, resolve flight carrierPitching angle thetaSWith roll angle γS. Suppose that current time speed is unchanged with respect to a upper moment speed,To acceleratingDegree fbxAnd fbyUtilize speed vyCompensate as follows:
f b x &prime; = f b x + &omega; ^ b z &CenterDot; v y = 0.1053 m / s 2 f b y &prime; = f b y - v &CenterDot; y = - 0.08 m / s 2 - - - ( 6 )
Wherein f 'bxAnd f 'byBeing respectively the linear acceleration and the centripetal acceleration that cause through the variable motion by flight carrier and rotation mendsAccekeration after repaying, utilizes the transformational relation of gravitational vectors between geographic coordinate system n system and body axis system b system, asks forRoll angle γsAnd pitching angle thetasFor:
Step 6: for course angle ψS, pitching angle thetaSAnd roll angle γS, utilize the relation between attitude quaternion and attitude angle,Ask for by geomagnetic fieldvector mbx,mby,mbzWith acceleration fbx,fby,fbzThe flight carrier attitude quaternion Q resolvingSFor:
Step 7: adopt Kalman Filter Technology, design Kalman filter, to utilizing angular velocity omegabx,ωby,ωbzThat resolves fliesRow attitude of carrier hypercomplex number QI, utilize geomagnetic fieldvector mbx,mby,mbzWith acceleration fbx,fby,fbzThe flight carrier attitude of resolvingHypercomplex number QS, through the angular speed after gyroscopic drift compensationThe data processing of carrying out Kalman filtering, estimatesUtilize angular velocity omegabx,ωby,ωbzThe flight carrier attitude quaternion Q resolvingICorresponding attitude error hypercomplex numberWith three axle speedGyroscopic drift errorAs follows:
1. set up system Kalman filtering state equation
Definition utilizes angular velocity omegabx,ωby,ωbzThe flight carrier attitude quaternion Q resolvingICorresponding attitude error hypercomplex number isWhereinThe system Kalman filtering state equation of foundation is:
X &CenterDot; ( t ) = F ( t ) &CenterDot; X ( t ) + G ( t ) &CenterDot; W ( t ) - - - ( 8 )
Wherein X (t) is the system mode vector in t moment, and F (t) and G (t) are t moment system mode transfer matrix and noise matrix,W (t) is system noise vector.
System mode vector: X (t)=[qe1 qe2 qe3 Δbx Δby Δbz]T
System noise vector: W (t)=[wgx wgy wgz wbx wby wbz]T
Wherein Δ bx、Δby、ΔbzBe three axle rate gyroscope drift estimate errors, wgx、wgy、wgzFor gyroscopic drift white noise, wbx、wby、wbzFor gyroscopic drift random walk white noise, system mode transfer matrix F (t) and noise matrix G (t) are:
F ( t ) = - &lsqb; &omega; ^ b &times; &rsqb; - 1 2 I 3 &times; 3 0 3 &times; 3 0 3 &times; 3 , G ( t ) = - 1 2 I 3 &times; 3 0 3 &times; 3 0 3 &times; 3 I 3 &times; 3
2. set up system Kalman filtering measurement equation
The system Kalman filtering measurement equation of foundation is:
Z(t)=H(t)X(t)+V(t) (9)
Wherein measurement matrix H (t)=[I3×3 03×3], V (t) is measurement noise, is approximately white noise.
Definition vector:
Y = Q S - 1 &CircleTimes; Q I - - - ( 10 )
System quantities measured value is:
Z ( t ) = Y ( 2 ) Y ( 3 ) Y ( 4 ) - - - ( 11 )
3. system Kalman filter equation discretization and state estimation
1. the system Kalman filter equation of and 2. setting up is carried out to discretization, obtain discretization Kalman filtering state equation and amountSurvey equation:
X k = &Phi; k , k - 1 X k - 1 + &Gamma; k - 1 W k - 1 Z k = H k X k + V k - - - ( 12 )
Wherein step transition matrix a: Фk,k-1=I6×6+T·F(tk), system noise drives battle array Гk-1=[I6×6+T/2·F(tk)]·G(tk) T, discretization cycle T=0.05s, system noise variance battle array QkWith measurement noise sidePoor battle array RkFor:
Qk=diag[(0.05°/s)2 (0.05°/s)2 (0.05°/s)2 (0.003°/s)2 (0.003°/s)2 (0.003°/s)2]
Rk=diag[0.00012 0.00012 0.00012]
To utilizing angular velocity omegabx,ωby,ωbzThe flight carrier attitude quaternion Q resolvingIWith geomagnetic fieldvector mbx,mby,mbzAnd accelerateDegree fbx,fby,fbzThe flight carrier attitude quaternion Q resolvingS, obtain Kalman filtering measuring value by (10) formulaAdopt following discrete type Kalman filtering fundamental equation to carry out filtering estimation:
P k / k - 1 = &Phi; k , k - 1 P k - 1 &Phi; k , k - 1 T + &Gamma; k - 1 Q k &Gamma; k - 1 T - - - ( 13 )
K k = P k / k - 1 H k T ( H k P k / k - 1 H k T + R k ) - 1 - - - ( 14 )
X ^ k = K k Z k - - - ( 15 )
P k = ( I - K k H k ) P k / k - 1 ( I - K k H k ) T + K k R k K k T - - - ( 16 )
Estimate to obtain utilizing after filtering angular velocity omegabx,ωby,ωbzThe flight carrier attitude quaternion Q resolvingICorresponding attitude errorHypercomplex number QIeEstimated valueWith three axle rate gyroscope drift estimate errorsFor:
Step 8: proofread and correct and utilize angular velocity omegabx,ωby,ωbzThe flight carrier attitude quaternion Q resolvingIAnd extraction flight carrier attitudeAngle, i.e. course angle ψ, pitching angle theta and roll angle γ. Utilize the attitude error hypercomplex number estimating in step 7Proofread and correct and utilizeAngular velocity omegabx,ωby,ωbzThe flight carrier attitude quaternion Q resolvingI, obtain current time flight carrier attitude quaternion optimum and estimateEvaluationFor:Utilization obtainsAccording to the attitude matrix of hypercomplex number form, obtain working asThe optimum attitude matrix in front momentNote:Extract course angle ψ, pitching angle theta andRoll angle γ is as follows:
Course angle:
&psi; = a tan ( T 12 / T 22 ) , T 22 > 0 &pi; + a tan ( T 12 / T 22 ) , T 22 < 0 , T 12 > 0 - &pi; + a tan ( T 12 / T 22 ) , T 22 < 0 , T 12 < 0 - - - ( 17 )
Roll angle:
&gamma; = arctan ( - T 31 / T 33 ) , T 33 > 0 &pi; + arctan ( - T 31 / T 33 ) , T 31 < 0 , T 33 < 0 - &pi; + arctan ( - T 31 / T 33 ) , T 31 > 0 , T 33 < 0 - - - ( 18 )
The angle of pitch:
θ=arcsin(T32) (19)
Asking for attitude angle is:
ψ=-0.0389°,θ=0.4844°,γ=0.5609°
The three axle rate gyroscope drift estimate errors of utilizing step 7 to obtainObtain current time three axle rate gyroscope drift estimatesValueAdoptProofread and correct three axle rate gyroscope measured values, thereby improve gyro to measure precision. WillAs the flight carrier attitude quaternion initial value in next moment, return to step 2 pick-up transducers signal, carry out next momentAttitude algorithm, cycle calculations, obtains the real-time attitude information of flight carrier.
In the present embodiment, adopt gyro, accelerometer, magnetometer and the velocity sensor combination of micro-electromechanical technology, can overcome listThe deficiency of individual sensor, effectively improves the definite precision of attitude, especially has very for the attitude measurement of microminiature flight carrierImportant meaning.

Claims (1)

1. flight carrier attitude is determined a method, it is characterized in that comprising the steps:
Step 1: coordinate system and the attitude matrix of setting up flight carrier: X in body axis system b systemb,Yb,ZbThree normal axis divideNot along the transverse axis of flight carrier to the right, before Y, vertical pivot upwards; Geographic coordinate system n system, Xn,Yn,ZnThree normal axis divideZhi Xiang east, north, sky; Attitude angle is defined as: course angle ψ is (180 °, 180 °), and pitching angle theta is (90 °, 90 °), rollAngle γ is (180 °, 180 °); According to the rotation order of course-pitching-roll, obtain the attitude square of the attitude angle form of flight carrierBattle arrayFor:
C n b = c &gamma; c &psi; + s &gamma; s &theta; s &psi; - c &gamma; s &psi; + s &gamma; s &theta; c &psi; - s &gamma; c &theta; c &theta; s &psi; c &theta; c &psi; s &theta; s &gamma; c &psi; - c &gamma; s &theta; s &psi; - s &gamma; s &psi; - c &gamma; s &theta; c &psi; c &gamma; c &theta; - - - ( 1 )
Wherein s and c are respectively writing a Chinese character in simplified form of function sin and cos, definition flight carrier attitude quaternion Q=[q0 q1 q2 q3]T,Obtain the attitude matrix of hypercomplex number formFor:
C n b = q 0 2 + q 1 2 - q 2 2 - q 3 2 2 ( q 1 q 2 + q 0 q 3 ) 2 ( q 1 q 3 - q 0 q 2 ) 2 ( q 1 q 2 - q 0 q 3 ) q 0 2 - q 1 2 + q 2 2 - q 3 2 2 ( q 2 q 3 + q 0 q 1 ) 2 ( q 1 q 3 + q 0 q 2 ) 2 ( q 2 q 3 - q 0 q 1 ) q 0 2 - q 1 2 - q 2 2 + q 3 2 - - - ( 2 )
Step 2: pick-up transducers signal: utilize gyro, accelerometer, individual axis velocity sensor to measure respectively flight carrierAngular speed, acceleration and speed, be converted to angular velocity omega through A/Db=[ωbx ωby ωbz]TAnd acceleration fb=[fbx fby fbz]TAnd speed vy, utilize magnetometer to obtain geomagnetic fieldvector mb=[mbx mby mbz]T
Step 3: utilize angular velocity omegab, adopt strapdown attitude derivation algorithm, ask for and utilize angular velocity omegabThe flight carrier appearance of resolvingState hypercomplex number QIAnd pitching angle thetaIAnd roll angle γI
Step 4: utilize geomagnetic fieldvector mbSolve the course angle ψ of flight carrierS: the pitching angle theta of utilizing step 3 to obtainIWithRoll angle γISet up the transition matrix between body axis system b system and horizontal coordinates h system, by geomagnetic fieldvector mbBe transformed into waterIn flat coordinate system h system, calculate the magnetic heading angle of flight carrier, through magnetic declination, correction obtains course angle ψS
Step 5: utilize speed vyTo acceleration fbCompensation, obtains the projection value of gravitational vectors in body axis system b system, profitAt the transformational relation between geographic coordinate system n system and body axis system b system, calculate the angle of pitch of flight carrier with gravitational vectors θSWith roll angle γS
To acceleration fbUtilize speed vyCompensate as follows:
f b x &prime; = f b x + &omega; ^ b z &CenterDot; v y f b y &prime; = f b y - v &CenterDot; y - - - ( 3 )
Wherein f 'bxAnd f 'byBeing respectively the linear acceleration and the centripetal acceleration that cause through the variable motion by flight carrier and rotation mendsAccekeration after repaying,For along ZbThe angular speed of the gyro of axle after gyroscopic drift compensation, vyFor along flight carrier bodyCoordinate system YbThe individual axis velocity measurement value sensor that axle is placed, asks for roll angle γSAnd pitching angle thetaSFor:
&gamma; S = a tan ( - f b x &prime; / f b z ) &theta; S = a tan ( ( - f b y &prime; / f b z ) sin&gamma; S ) - - - ( 4 )
Step 6: for course angle ψS, pitching angle thetaSAnd roll angle γS, utilize the relation between attitude quaternion and attitude angle,Ask for by geomagnetic fieldvector mbWith acceleration fbThe flight carrier attitude quaternion Q resolvingS
Step 7: adopt Kalman Filter Technology, design Kalman filter, to utilizing angular velocity omegabThe flight carrier appearance of resolvingState hypercomplex number QI, utilize geomagnetic fieldvector mbWith acceleration fbThe flight carrier attitude quaternion Q resolvingS, gyro to measure value ωb,Carry out the data processing of Kalman filtering, estimate and utilize angular velocity omegabThe flight carrier attitude quaternion Q resolvingICorresponding attitudeError quaternionWith gyroscopic drift errorComprise following sub-step:
1. set up system Kalman filtering state equation
Gyro to measure model is established as: ωb=ω+b+wg, wherein ωbFor gyro output valve, ω is true angular speed, wgForGyroscopic drift white noise, b is gyroscopic drift, meets equationFor gyroscopic drift random walk white noise;
Definition utilizes angular velocity omegabThe flight carrier attitude quaternion Q resolvingICorresponding attitude error hypercomplex number isWhereinReal flight carrier attitude quaternion is Q, and Q can be expressed as:
Q = Q I &CircleTimes; Q I e - - - ( 5 )
Differentiated in (5) formula both sides:
Q &CenterDot; = Q &CenterDot; I &CircleTimes; Q I e + Q I &CircleTimes; Q &CenterDot; I e - - - ( 6 )
Q &CenterDot; I e = 1 2 Q I e &CircleTimes; &omega; b - 1 2 &omega; ^ b &CircleTimes; Q I e - - - ( 7 )
:
q &RightArrow; &CenterDot; e = - &lsqb; &omega; ^ b &times; &rsqb; &CenterDot; q &RightArrow; e - 1 2 &Delta; b - 1 2 w g - - - ( 8 )
WhereinFor gyroscopic drift evaluated error, b is that gyro truly drifts about,For gyroscopic drift estimated value,ForSkew symmetric matrix,For angular velocity omegabValue after gyroscopic drift compensation, sets up system Kalman filtering state equationFor:
X &CenterDot; ( t ) = F ( t ) &CenterDot; ( t ) + G ( t ) &CenterDot; W ( t ) - - - ( 9 )
Wherein X (t) is the system mode vector in t moment, and F (t) and G (t) are t moment system mode transfer matrix and noise matrix,W (t) is system noise vector;
System mode vector: X (t)=[qe1 qe2 qe3 Δbx Δby Δbz]T
System noise vector: W (t)=[wgx wgy wgz wbx wby wbz]T
Wherein qe1、qe2、qe3For attitude error hypercomplex number QIeThree components, Δ bx、Δby、ΔbzFor gyroscopic drift is estimatedError, wgx、wgy、wgzFor gyroscopic drift white noise, wbx、wby、wbzFor gyroscopic drift random walk white noise, systemState-transition matrix F (t) and noise matrix G (t) are:
F ( t ) = - &lsqb; &omega; ^ b &times; &rsqb; - 1 2 I 3 &times; 3 0 3 &times; 3 0 3 &times; 3 , G ( t ) = - 1 2 I 3 &times; 3 0 3 &times; 3 0 3 &times; 3 I 3 &times; 3
2. set up system Kalman filtering measurement equation
Definition utilizes geomagnetic fieldvector mbWith acceleration fbThe flight carrier attitude quaternion Q resolvingSCorresponding attitude error quaternaryNumber is QSe=[1 qse1 qse2 qse3]T:
Q = Q I &CircleTimes; Q I e Q = Q S &CircleTimes; Q S e - - - ( 10 )
Definition vectorObtained by formula (10):
Y = Q S - 1 &CircleTimes; Q I = Q S e &CircleTimes; Q I e - 1 = 1 - q s e 1 - q s e 2 - q s e 3 q s e 1 1 - q s e 3 q s e 2 q s e 2 q s e 3 1 - q s e 1 q s e 3 - q s e 2 q s e 1 1 &CenterDot; 1 - q e 1 - q e 2 - q e 3 - - - ( 11 )
Ignore second order term, the system Kalman filtering measurement equation of obtaining is:
Z ( t ) = Y ( 2 ) Y ( 3 ) Y ( 4 ) = q s e 1 - q e 1 q s e 2 - q e 2 q s e 3 - q e 3 = H ( t ) X ( t ) + V ( t ) - - - ( 12 )
Wherein measurement matrix H (t)=[I3×3 03×3], V (t) is measurement noise, is approximately white noise;
3. system Kalman filter equation discretization and state estimation
1. the system Kalman filter equation of and 2. setting up is carried out to discretization, obtain discretization Kalman filtering state equation and amountSurvey equation is:
X k = &Phi; k , k - 1 X k - 1 + &Gamma; k - 1 W k - 1 Z k = H k X k + V k - - - ( 13 )
A wherein step transition matrixSystem noise drives battle arrayT is the discretization cycle, system noise variance battle array QkWith measuring noise square difference battle array RkFor:
Q k = d i a g &sigma; g x 2 &sigma; g y 2 &sigma; g z 2 &sigma; b x 2 &sigma; b x 2 &sigma; b x 2 - - - ( 14 ) R k = d i a g &delta;q e 1 2 &delta;q e 2 2 &delta;q e 3 2 - - - ( 15 )
Wherein σgx,σgy,σgzFor gyroscopic drift white noise root mean square, σbx,σby,σbzFor all sides of gyroscopic drift random walk white noiseRoot, δ qe1,δqe2,δqe3For flight carrier attitude quaternion evaluated error, to utilizing angular velocity omegabThe flight carrier attitude four of resolvingThe number Q of unitIWith geomagnetic fieldvector mbAnd acceleration fbThe flight carrier attitude quaternion Q resolvingS, by (11) formula computer card GermaniaFiltering measuring value Zk, adopt discrete type Kalman filtering fundamental equation to carry out filtering estimation, obtain utilizing angular velocity omegabThat resolves fliesRow attitude of carrier hypercomplex number QICorresponding attitude error hypercomplex number QIeEstimated valueAnd gyroscopic drift evaluated error
Step 8: proofread and correct and utilize angular velocity omegabThe flight carrier attitude quaternion Q resolvingIAnd extract flight carrier attitude angle, navigateTo angle ψ, pitching angle theta and roll angle γ; Utilize the attitude error hypercomplex number estimating in step 7Correction utilizes angular velocity omegabThe flight carrier attitude quaternion Q resolvingI, obtain current time flight carrier attitude quaternion optimal estimation value
Utilization obtainsObtain corresponding attitude matrix, utilize attitude matrix to extract flight carrier attitude angle, course angle ψ,Pitching angle theta and roll angle γ; To the gyroscopic drift evaluated error obtaining through Kalman filteringObtain gyroscopic drift estimated valueAdoptProofread and correct gyro to measure value, thereby improve gyro to measure precision; WillAs next moment attitude algorithm hypercomplex numberInitial value, returns to step 2 pick-up transducers signal, carries out the attitude algorithm in next moment, cycle calculations.
CN200810076417.0A 2008-08-18 2008-08-18 A kind of flight carrier attitude is determined method CN106342284B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN200810076417.0A CN106342284B (en) 2008-08-18 2008-08-18 A kind of flight carrier attitude is determined method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN200810076417.0A CN106342284B (en) 2008-08-18 2008-08-18 A kind of flight carrier attitude is determined method

Publications (1)

Publication Number Publication Date
CN106342284B true CN106342284B (en) 2011-11-23

Family

ID=57797860

Family Applications (1)

Application Number Title Priority Date Filing Date
CN200810076417.0A CN106342284B (en) 2008-08-18 2008-08-18 A kind of flight carrier attitude is determined method

Country Status (1)

Country Link
CN (1) CN106342284B (en)

Cited By (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106979779A (en) * 2017-05-22 2017-07-25 深圳市靖洲科技有限公司 A kind of unmanned vehicle real-time attitude measuring method
CN107063173A (en) * 2017-06-13 2017-08-18 广州辛群科技有限公司 Angle detecting method and joint motions angle detecting system
CN107117280A (en) * 2017-05-11 2017-09-01 南方科技大学 Dirigible landing control method and device
CN107131865A (en) * 2017-06-13 2017-09-05 广州辛群科技有限公司 Angle detection device
CN107272718A (en) * 2017-06-19 2017-10-20 歌尔科技有限公司 Attitude control method and device based on Kalman filtering
CN107478223A (en) * 2016-06-08 2017-12-15 南京理工大学 A kind of human body attitude calculation method based on quaternary number and Kalman filtering
CN107817821A (en) * 2017-10-27 2018-03-20 成都鼎信精控科技有限公司 A kind of stable head and control method based on MEMS gyroscope combination
CN108318038A (en) * 2018-01-26 2018-07-24 南京航空航天大学 A kind of quaternary number Gaussian particle filtering pose of mobile robot calculation method
CN108475066A (en) * 2017-04-21 2018-08-31 深圳市大疆创新科技有限公司 Unmanned vehicle computation method for attitude, flight controller and unmanned vehicle
CN108805175A (en) * 2018-05-21 2018-11-13 郑州大学 A kind of flight attitude clustering method of aircraft and analysis system
CN108955671A (en) * 2018-04-25 2018-12-07 珠海全志科技股份有限公司 A kind of Kalman filtering air navigation aid based on magnetic declination, magnetic dip angle
CN109030867A (en) * 2017-06-08 2018-12-18 海智芯株式会社 The method and apparatus for calculating angular speed using acceleration transducer and geomagnetic sensor
CN109163721A (en) * 2018-09-18 2019-01-08 河北美泰电子科技有限公司 Attitude measurement method and terminal device
CN109883444A (en) * 2019-02-25 2019-06-14 航天科工防御技术研究试验中心 A kind of attitude angle coupling error compensation method, device and electronic equipment
CN110440797A (en) * 2019-08-28 2019-11-12 广州小鹏汽车科技有限公司 Vehicle attitude estimation method and system
CN110887480A (en) * 2019-12-11 2020-03-17 中国空气动力研究与发展中心低速空气动力研究所 Flight attitude estimation method and system based on MEMS sensor
WO2021031974A1 (en) * 2019-08-19 2021-02-25 深圳市道通智能航空技术有限公司 Method for selecting initial value of course angle of unmanned aerial vehicle and unmanned aerial vehicle

Cited By (19)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107478223A (en) * 2016-06-08 2017-12-15 南京理工大学 A kind of human body attitude calculation method based on quaternary number and Kalman filtering
CN108475066A (en) * 2017-04-21 2018-08-31 深圳市大疆创新科技有限公司 Unmanned vehicle computation method for attitude, flight controller and unmanned vehicle
CN108475066B (en) * 2017-04-21 2021-02-19 深圳市大疆创新科技有限公司 Unmanned aerial vehicle attitude calculation method, flight controller and unmanned aerial vehicle
CN107117280A (en) * 2017-05-11 2017-09-01 南方科技大学 Dirigible landing control method and device
CN106979779A (en) * 2017-05-22 2017-07-25 深圳市靖洲科技有限公司 A kind of unmanned vehicle real-time attitude measuring method
CN109030867A (en) * 2017-06-08 2018-12-18 海智芯株式会社 The method and apparatus for calculating angular speed using acceleration transducer and geomagnetic sensor
CN107131865A (en) * 2017-06-13 2017-09-05 广州辛群科技有限公司 Angle detection device
CN107063173A (en) * 2017-06-13 2017-08-18 广州辛群科技有限公司 Angle detecting method and joint motions angle detecting system
CN107272718B (en) * 2017-06-19 2020-07-24 歌尔科技有限公司 Attitude control method and device based on Kalman filtering
CN107272718A (en) * 2017-06-19 2017-10-20 歌尔科技有限公司 Attitude control method and device based on Kalman filtering
CN107817821A (en) * 2017-10-27 2018-03-20 成都鼎信精控科技有限公司 A kind of stable head and control method based on MEMS gyroscope combination
CN108318038A (en) * 2018-01-26 2018-07-24 南京航空航天大学 A kind of quaternary number Gaussian particle filtering pose of mobile robot calculation method
CN108955671A (en) * 2018-04-25 2018-12-07 珠海全志科技股份有限公司 A kind of Kalman filtering air navigation aid based on magnetic declination, magnetic dip angle
CN108805175A (en) * 2018-05-21 2018-11-13 郑州大学 A kind of flight attitude clustering method of aircraft and analysis system
CN109163721A (en) * 2018-09-18 2019-01-08 河北美泰电子科技有限公司 Attitude measurement method and terminal device
CN109883444A (en) * 2019-02-25 2019-06-14 航天科工防御技术研究试验中心 A kind of attitude angle coupling error compensation method, device and electronic equipment
WO2021031974A1 (en) * 2019-08-19 2021-02-25 深圳市道通智能航空技术有限公司 Method for selecting initial value of course angle of unmanned aerial vehicle and unmanned aerial vehicle
CN110440797A (en) * 2019-08-28 2019-11-12 广州小鹏汽车科技有限公司 Vehicle attitude estimation method and system
CN110887480A (en) * 2019-12-11 2020-03-17 中国空气动力研究与发展中心低速空气动力研究所 Flight attitude estimation method and system based on MEMS sensor

Similar Documents

Publication Publication Date Title
Wu et al. Velocity/position integration formula part I: Application to in-flight coarse alignment
CN103245360B (en) Carrier-borne aircraft rotation type strapdown inertial navigation system Alignment Method under swaying base
Gao et al. Rapid fine strapdown INS alignment method under marine mooring condition
EP2557394B1 (en) System for processing pulse signals within an inertial navigation system
CN104898681B (en) A kind of quadrotor attitude acquisition method for approximately finishing card quaternary number using three ranks
KR101168100B1 (en) Systems and methods for estimating position, attitude and/or heading of a vehicle
CN100585602C (en) Inertial measuring system error model demonstration test method
Han et al. A novel method to integrate IMU and magnetometers in attitude and heading reference systems
US8005635B2 (en) Self-calibrated azimuth and attitude accuracy enhancing method and system (SAAAEMS)
CN101788296B (en) SINS/CNS deep integrated navigation system and realization method thereof
CN103323026B (en) The attitude reference estimation of deviation of star sensor and useful load and modification method
CN100405014C (en) Carrier attitude measurement method and system
CN1740746B (en) Micro-dynamic carrier attitude measuring apparatus and measuring method thereof
CN105606094B (en) A kind of information condition matched filtering method of estimation based on MEMS/GPS combined systems
CN101344391B (en) Lunar vehicle posture self-confirming method based on full-function sun-compass
CN101726295B (en) Unscented Kalman filter-based method for tracking inertial pose according to acceleration compensation
CN101514899B (en) Optical fibre gyro strapdown inertial navigation system error inhibiting method based on single-shaft rotation
Zhou et al. Integrated navigation system for a low-cost quadrotor aerial vehicle in the presence of rotor influences
CN101706281B (en) Inertia/astronomy/satellite high-precision integrated navigation system and navigation method thereof
Huang et al. Kalman-filtering-based in-motion coarse alignment for odometer-aided SINS
CN102692225B (en) Attitude heading reference system for low-cost small unmanned aerial vehicle
CN103776446B (en) A kind of pedestrian&#39;s independent navigation computation based on double MEMS-IMU
CN102980577B (en) Micro-strapdown altitude heading reference system and working method thereof
CN100356139C (en) Miniature assembled gesture measuring system for mini-satellite
CN104406586A (en) Pedestrian navigation device and pedestrian navigation method based on inertial sensor

Legal Events

Date Code Title Description
DC01 Secret patent status has been lifted
DCSP Declassification of secret patent
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20111123

Termination date: 20180818

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