CN106342284B  A kind of flight carrier attitude is determined method  Google Patents
A kind of flight carrier attitude is determined methodInfo
 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
Links
 239000000969 carrier Substances 0.000 title claims abstract description 100
 239000011159 matrix material Substances 0.000 claims abstract description 43
 230000001133 acceleration Effects 0.000 claims abstract description 36
 238000005259 measurement Methods 0.000 claims abstract description 22
 238000001914 filtration Methods 0.000 claims description 31
 230000000875 corresponding Effects 0.000 claims description 13
 235000020127 ayran Nutrition 0.000 claims description 9
 238000005295 random walk Methods 0.000 claims description 7
 239000004576 sand Substances 0.000 claims description 6
 238000009795 derivation Methods 0.000 claims description 3
 239000000203 mixture Substances 0.000 abstract description 4
 238000009825 accumulation Methods 0.000 description 1
 238000006243 chemical reaction Methods 0.000 description 1
 239000002131 composite material Substances 0.000 description 1
 238000010586 diagram Methods 0.000 description 1
 230000000694 effects Effects 0.000 description 1
 238000000605 extraction Methods 0.000 description 1
 230000003068 static Effects 0.000 description 1
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; Pickup 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 realtime 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
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 longtime 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 motordriven 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 motordriven 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 motordriven 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 abovementioned 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 system_{b}，Y_{b}，Z_{b}ThreeIndividual normal axis respectively along the transverse axis of flight carrier to the right, before Y, vertical pivot upwards. Geographic coordinate system n system, X_{n}，Y_{n}，Z_{n}ThreeIndividual 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 coursepitchingroll, obtain the appearance of the attitude angle form of flight carrierState matrixFor:
Wherein s and c are respectively writing a Chinese character in simplified form of function sin and cos, definition flight carrier attitude quaternion Q=[q_{0} q_{1} q_{2} q_{3}]^{T}，Obtain the attitude matrix of hypercomplex number formFor:
Step 2: pickup 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/D_{b}＝[ω_{bx} ω_{by} ω_{bz}]^{T}And acceleration
f_{b}＝[f_{bx} f_{by} f_{bz}]^{T}And speed v_{y}, utilize magnetometer to obtain geomagnetic fieldvector m_{b}＝[m_{bx} m_{by} m_{bz}]^{T}。
Step 3: utilize angular velocity omega_{b}, adopt strapdown attitude derivation algorithm, ask for and utilize angular velocity omega_{b}The flight carrier appearance of resolvingState hypercomplex number Q_{I}And pitching angle theta_{I}And roll angle γ_{I}。
Step 4: utilize geomagnetic fieldvector m_{b}Solve the course angle ψ of flight carrier_{S}. The pitching angle theta of utilizing step 3 to obtain_{I}WithRoll angle γ_{I}Set up the transition matrix between body axis system b system and horizontal coordinates h system, by geomagnetic fieldvector m_{b}Be 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 v_{y}To acceleration f_{b}Compensation, 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
θ_{S}With roll angle γ_{S}。
When flight carrier is under state static or that acceleration is very little, ignore the impact of acceleration, acceleration f_{b}Be 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 f_{b}To no longer reflect real gravitational vectors, therefore, to acceleration f_{b}Utilize speed v_{y}CompensationAs follows:
Wherein f '_{bx}And f '_{by}Being respectively the linear acceleration and the centripetal acceleration that cause through the variable motion by flight carrier and rotation mendsAccekeration after repaying,For along Z_{b}The angular speed of the gyro of axle after gyroscopic drift compensation, v_{y}For along flight carrier bodyCoordinate system Y_{b}The individual axis velocity measurement value sensor that axle is placed, asks for roll angle γ_{S}And pitching angle theta_{S}For:
Step 6: for course angle ψ_{S}, pitching angle theta_{S}And roll angle γ_{S}, utilize the relation between attitude quaternion and attitude angle,Ask for by geomagnetic fieldvector m_{b}With acceleration f_{b}The flight carrier attitude quaternion Q resolving_{S}。
Step 7: adopt Kalman Filter Technology, design Kalman filter, to utilizing angular velocity omega_{b}The flight carrier appearance of resolvingState hypercomplex number Q_{I}, utilize geomagnetic fieldvector m_{b}With acceleration f_{b}The flight carrier attitude quaternion Q resolving_{S}, gyro to measure value ω_{b}，Carry out the data processing of Kalman filtering, estimate and utilize angular velocity omega_{b}The flight carrier attitude quaternion Q resolving_{I}Corresponding attitudeError quaternionWith gyroscopic drift errorComprise following substep:
1. set up system Kalman filtering state equation
Gyro to measure model is established as: ω_{b}＝ω+b+w_{g}, wherein ω_{b}For gyro output valve, ω is true angular speed, w_{g}ForGyroscopic drift white noise, b is gyroscopic drift, meets equationw_{b}For gyroscopic drift random walk white noise
Definition utilizes angular velocity omega_{b}The flight carrier attitude quaternion Q resolving_{I}Corresponding attitude error hypercomplex number isWhereinReal flight carrier attitude quaternion is Q, and Q can be expressed as:
Differentiated in (5) formula both sides:
:
WhereinFor gyroscopic drift evaluated error, b is that gyro truly drifts about,For gyroscopic drift estimated value,ForSkew symmetric matrix,For angular velocity omega_{b}Value after gyroscopic drift compensation, sets up system Kalman filtering state equationFor:
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)=[q_{e1} q_{e2} q_{e3} Δb_{x} Δb_{y} Δb_{z}]^{T}
System noise vector: W (t)=[w_{gx} w_{gy} w_{gz} w_{bx} w_{by} w_{bz}]^{T}
Wherein q_{e1}、q_{e2}、q_{e3}For attitude error hypercomplex number Q_{Ie}Three components, Δ b_{x}、Δb_{y}、Δb_{z}For gyroscopic drift is estimatedError, w_{gx}、w_{gy}、w_{gz}For gyroscopic drift white noise, w_{bx}、w_{by}、w_{bz}For gyroscopic drift random walk white noise, systemStatetransition matrix F (t) and noise matrix G (t) are:
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 m_{b}With acceleration f_{b}The flight carrier attitude quaternion Q resolving_{S}Corresponding attitude error quaternaryNumber is Q_{Se}＝[1 q_{se1} q_{se2} q_{se3}]^{T}:
Definition vectorObtained by formula (10):
Ignore second order term, the system Kalman filtering measurement equation of obtaining is:
Wherein measurement matrix H (t)=[I_{3×3} 0_{3×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:
A wherein step transition matrixSystem noise drives battle arrayT is the discretization cycle, system noise variance battle array Q_{k}With measuring noise square difference battle array R_{k}For:
R_{k}＝diag[δq_{e1} ^{2} δq_{e2} ^{2} δq_{e3} ^{2}] (15)
Wherein σ_{gx}，σ_{gy}，σ_{gz}For gyroscopic drift white noise root mean square, σ_{bx}，σ_{by}，σ_{bz}For all sides of gyroscopic drift random walk white noiseRoot, δ q_{e1}，δq_{e2}，δq_{e3}For flight carrier attitude quaternion evaluated error, to utilizing angular velocity omega_{b}The flight carrier attitude four of resolvingThe number Q of unit_{I}With geomagnetic fieldvector m_{b}And acceleration f_{b}The flight carrier attitude quaternion Q resolving_{S}, by (11) formula computer card GermaniaFiltering measuring value Z_{k}, adopt discrete type Kalman filtering fundamental equation to carry out filtering estimation, obtain utilizing angular velocity omega_{b}That resolves fliesRow attitude of carrier hypercomplex number Q_{I}Corresponding attitude error hypercomplex number Q_{Ie}Estimated valueAnd gyroscopic drift evaluated error
Step 8: proofread and correct and utilize angular velocity omega_{b}The flight carrier attitude quaternion Q resolving_{I}And 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 omega_{b}The flight carrier attitude quaternion Q resolving_{I}, 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 pickup transducers signal, carries out the attitude algorithm in next moment, cycle calculations.
The invention has the beneficial effects as follows: 1) by gyro, accelerometer, micromagnetometer, velocity sensor combination, thereby metThe requirement of flight carrier realtime 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 microelectromechanical technologyVelocity sensor is as system sensor, and pickup 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 system_{b}，Y_{b}，Z_{b}ThreeIndividual normal axis respectively along the transverse axis of flight carrier to the right, before Y, vertical pivot upwards. X in geographic coordinate system n system_{n}，Y_{n}，Z_{n}ThreeIndividual 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 Y_{b}Axle 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:
Definition flight carrier attitude quaternion Q=[q_{0} q_{1} q_{2} q_{3}]^{T}, the attitude matrix of hypercomplex number formFor:
Step 2: pickup 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 omega_{bx}＝0.01°/s、ω_{by}＝0.02°/s、ω_{bz}=0.03 °/s, acceleration f_{bx}＝0.08m/s^{2}、
f_{by}＝0.08m/s^{2}、f_{bz}＝9.76m/s^{2}, speed v_{y}=50m/s, utilizes three axis magnetometer to obtain geomagnetic fieldvector
m_{bx}＝80、m_{by}＝3500、m_{bz}=4500, wherein conversion coefficient is 15000/Gs.
Step 3: the angular velocity omega that utilizes three axle rate gyroscopes to measure_{bx}，ω_{by}，ω_{bz}, adopt strapdown attitude derivation algorithm, ask for profitUse angular velocity omega_{b}The flight carrier attitude quaternion Q resolving_{I}And pitching angle theta_{I}And 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 omega_{bx}，ω_{by}，ω_{bz}Through topAfter spiral shell drift compensation, be:
WhereinFor the angular speed after gyroscopic drift compensation, utilizeSolve by strapdown attitudeArrive flight carrier by angular velocity omega_{bx}，ω_{by}，ω_{bz}The flight carrier attitude quaternion Q resolving_{I}And pitching angle theta_{I}And roll angle γ_{I}For:
θ_{I}＝0.50046°，γ_{I}＝0.50094°
Step 4: utilize geomagnetic fieldvector to solve the course angle ψ of flight carrier_{S}. The pitching angle theta of utilizing step 3 to obtain_{I}And rollAngle γ_{I}Set up the transition matrix between body axis system b system and horizontal coordinates h system, by geomagnetic fieldvector m_{bx}，m_{by}Be transformed into waterIn flat coordinate system h system:
Wherein m_{bx}，m_{by}For geomagnetic fieldvector m_{bx}，m_{by}Be transformed into the value in horizontal coordinates h system, ask for magnetic heading angle ψ_{m}For:
Through magnetic declination, correction obtains the flight carrier course angle ψ that utilizes geomagnetic fieldvector to solve_{S}＝0.552°
Step 5: the speed v of utilizing individual axis velocity sensor to obtain_{y}To acceleration f_{bx}，f_{by}Compensate, resolve flight carrierPitching angle theta_{S}With roll angle γ_{S}. Suppose that current time speed is unchanged with respect to a upper moment speed,To acceleratingDegree f_{bx}And f_{by}Utilize speed v_{y}Compensate as follows:
Wherein f '_{bx}And f '_{by}Being 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 γ_{s}And pitching angle theta_{s}For:
Step 6: for course angle ψ_{S}, pitching angle theta_{S}And roll angle γ_{S}, utilize the relation between attitude quaternion and attitude angle,Ask for by geomagnetic fieldvector m_{bx}，m_{by}，m_{bz}With acceleration f_{bx}，f_{by}，f_{bz}The flight carrier attitude quaternion Q resolving_{S}For:
Step 7: adopt Kalman Filter Technology, design Kalman filter, to utilizing angular velocity omega_{bx}，ω_{by}，ω_{bz}That resolves fliesRow attitude of carrier hypercomplex number Q_{I}, utilize geomagnetic fieldvector m_{bx}，m_{by}，m_{bz}With acceleration f_{bx}，f_{by}，f_{bz}The flight carrier attitude of resolvingHypercomplex number Q_{S}, through the angular speed after gyroscopic drift compensationThe data processing of carrying out Kalman filtering, estimatesUtilize angular velocity omega_{bx}，ω_{by}，ω_{bz}The flight carrier attitude quaternion Q resolving_{I}Corresponding attitude error hypercomplex numberWith three axle speedGyroscopic drift errorAs follows:
1. set up system Kalman filtering state equation
Definition utilizes angular velocity omega_{bx}，ω_{by}，ω_{bz}The flight carrier attitude quaternion Q resolving_{I}Corresponding attitude error hypercomplex number isWhereinThe system Kalman filtering state equation of foundation is:
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)=[q_{e1} q_{e2} q_{e3} Δb_{x} Δb_{y} Δb_{z}]^{T}
System noise vector: W (t)=[w_{gx} w_{gy} w_{gz} w_{bx} w_{by} w_{bz}]^{T}
Wherein Δ b_{x}、Δb_{y}、Δb_{z}Be three axle rate gyroscope drift estimate errors, w_{gx}、w_{gy}、w_{gz}For gyroscopic drift white noise,
w_{bx}、w_{by}、w_{bz}For gyroscopic drift random walk white noise, system mode transfer matrix F (t) and noise matrix G (t) are:
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)=[I_{3×3} 0_{3×3}], V (t) is measurement noise, is approximately white noise.
Definition vector:
System quantities measured value is:
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:
Wherein step transition matrix a: Ф_{k，k1}＝I_{6×6}+T·F(t_{k}), system noise drives battle array
Г_{k1}＝[I_{6×6}+T/2·F(t_{k})]·G(t_{k}) T, discretization cycle T=0.05s, system noise variance battle array Q_{k}With measurement noise sidePoor battle array R_{k}For:
Q_{k}＝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}]
R_{k}＝diag[0.0001^{2} 0.0001^{2} 0.0001^{2}]
To utilizing angular velocity omega_{bx}，ω_{by}，ω_{bz}The flight carrier attitude quaternion Q resolving_{I}With geomagnetic fieldvector m_{bx}，m_{by}，m_{bz}And accelerateDegree f_{bx}，f_{by}，f_{bz}The flight carrier attitude quaternion Q resolving_{S}, obtain Kalman filtering measuring value by (10) formulaAdopt following discrete type Kalman filtering fundamental equation to carry out filtering estimation:
Estimate to obtain utilizing after filtering angular velocity omega_{bx}，ω_{by}，ω_{bz}The flight carrier attitude quaternion Q resolving_{I}Corresponding attitude errorHypercomplex number Q_{Ie}Estimated valueWith three axle rate gyroscope drift estimate errorsFor:
Step 8: proofread and correct and utilize angular velocity omega_{bx}，ω_{by}，ω_{bz}The flight carrier attitude quaternion Q resolving_{I}And 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 omega_{bx}，ω_{by}，ω_{bz}The flight carrier attitude quaternion Q resolving_{I}, 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:
Roll angle:
The angle of pitch:
θ＝arcsin(T_{32}) (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 pickup transducers signal, carry out next momentAttitude algorithm, cycle calculations, obtains the realtime attitude information of flight carrier.
In the present embodiment, adopt gyro, accelerometer, magnetometer and the velocity sensor combination of microelectromechanical 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 system_{b}，Y_{b}，Z_{b}Three normal axis divideNot along the transverse axis of flight carrier to the right, before Y, vertical pivot upwards; Geographic coordinate system n system, X_{n}，Y_{n}，Z_{n}Three 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 coursepitchingroll, obtain the attitude square of the attitude angle form of flight carrierBattle arrayFor:
Wherein s and c are respectively writing a Chinese character in simplified form of function sin and cos, definition flight carrier attitude quaternion Q=[q_{0} q_{1} q_{2} q_{3}]^{T}，Obtain the attitude matrix of hypercomplex number formFor:
Step 2: pickup 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/D_{b}＝[ω_{bx} ω_{by} ω_{bz}]^{T}And acceleration
f_{b}＝[f_{bx} f_{by} f_{bz}]^{T}And speed v_{y}, utilize magnetometer to obtain geomagnetic fieldvector m_{b}＝[m_{bx} m_{by} m_{bz}]^{T}；
Step 3: utilize angular velocity omega_{b}, adopt strapdown attitude derivation algorithm, ask for and utilize angular velocity omega_{b}The flight carrier appearance of resolvingState hypercomplex number Q_{I}And pitching angle theta_{I}And roll angle γ_{I}；
Step 4: utilize geomagnetic fieldvector m_{b}Solve the course angle ψ of flight carrier_{S}: the pitching angle theta of utilizing step 3 to obtain_{I}WithRoll angle γ_{I}Set up the transition matrix between body axis system b system and horizontal coordinates h system, by geomagnetic fieldvector m_{b}Be 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 v_{y}To acceleration f_{b}Compensation, 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
θ_{S}With roll angle γ_{S}；
To acceleration f_{b}Utilize speed v_{y}Compensate as follows:
Wherein f '_{bx}And f '_{by}Being respectively the linear acceleration and the centripetal acceleration that cause through the variable motion by flight carrier and rotation mendsAccekeration after repaying,For along Z_{b}The angular speed of the gyro of axle after gyroscopic drift compensation, v_{y}For along flight carrier bodyCoordinate system Y_{b}The individual axis velocity measurement value sensor that axle is placed, asks for roll angle γ_{S}And pitching angle theta_{S}For:
Step 6: for course angle ψ_{S}, pitching angle theta_{S}And roll angle γ_{S}, utilize the relation between attitude quaternion and attitude angle,Ask for by geomagnetic fieldvector m_{b}With acceleration f_{b}The flight carrier attitude quaternion Q resolving_{S}；
Step 7: adopt Kalman Filter Technology, design Kalman filter, to utilizing angular velocity omega_{b}The flight carrier appearance of resolvingState hypercomplex number Q_{I}, utilize geomagnetic fieldvector m_{b}With acceleration f_{b}The flight carrier attitude quaternion Q resolving_{S}, gyro to measure value ω_{b}，Carry out the data processing of Kalman filtering, estimate and utilize angular velocity omega_{b}The flight carrier attitude quaternion Q resolving_{I}Corresponding attitudeError quaternionWith gyroscopic drift errorComprise following substep:
1. set up system Kalman filtering state equation
Gyro to measure model is established as: ω_{b}＝ω+b+w_{g}, wherein ω_{b}For gyro output valve, ω is true angular speed, w_{g}ForGyroscopic drift white noise, b is gyroscopic drift, meets equationFor gyroscopic drift random walk white noise;
Definition utilizes angular velocity omega_{b}The flight carrier attitude quaternion Q resolving_{I}Corresponding attitude error hypercomplex number isWhereinReal flight carrier attitude quaternion is Q, and Q can be expressed as:
Differentiated in (5) formula both sides:
:
WhereinFor gyroscopic drift evaluated error, b is that gyro truly drifts about,For gyroscopic drift estimated value,ForSkew symmetric matrix,For angular velocity omega_{b}Value after gyroscopic drift compensation, sets up system Kalman filtering state equationFor:
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)=[q_{e1} q_{e2} q_{e3} Δb_{x} Δb_{y} Δb_{z}]^{T}
System noise vector: W (t)=[w_{gx} w_{gy} w_{gz} w_{bx} w_{by} w_{bz}]^{T}
Wherein q_{e1}、q_{e2}、q_{e3}For attitude error hypercomplex number Q_{Ie}Three components, Δ b_{x}、Δb_{y}、Δb_{z}For gyroscopic drift is estimatedError, w_{gx}、w_{gy}、w_{gz}For gyroscopic drift white noise, w_{bx}、w_{by}、w_{bz}For gyroscopic drift random walk white noise, systemStatetransition matrix F (t) and noise matrix G (t) are:
2. set up system Kalman filtering measurement equation
Definition utilizes geomagnetic fieldvector m_{b}With acceleration f_{b}The flight carrier attitude quaternion Q resolving_{S}Corresponding attitude error quaternaryNumber is Q_{Se}＝[1 q_{se1} q_{se2} q_{se3}]^{T}:
Definition vectorObtained by formula (10):
Ignore second order term, the system Kalman filtering measurement equation of obtaining is:
Wherein measurement matrix H (t)=[I_{3×3} 0_{3×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:
A wherein step transition matrixSystem noise drives battle arrayT is the discretization cycle, system noise variance battle array Q_{k}With measuring noise square difference battle array R_{k}For:
Wherein σ_{gx}，σ_{gy}，σ_{gz}For gyroscopic drift white noise root mean square, σ_{bx}，σ_{by}，σ_{bz}For all sides of gyroscopic drift random walk white noiseRoot, δ q_{e1}，δq_{e2}，δq_{e3}For flight carrier attitude quaternion evaluated error, to utilizing angular velocity omega_{b}The flight carrier attitude four of resolvingThe number Q of unit_{I}With geomagnetic fieldvector m_{b}And acceleration f_{b}The flight carrier attitude quaternion Q resolving_{S}, by (11) formula computer card GermaniaFiltering measuring value Z_{k}, adopt discrete type Kalman filtering fundamental equation to carry out filtering estimation, obtain utilizing angular velocity omega_{b}That resolves fliesRow attitude of carrier hypercomplex number Q_{I}Corresponding attitude error hypercomplex number Q_{Ie}Estimated valueAnd gyroscopic drift evaluated error
Step 8: proofread and correct and utilize angular velocity omega_{b}The flight carrier attitude quaternion Q resolving_{I}And 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 omega_{b}The flight carrier attitude quaternion Q resolving_{I}, 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 pickup transducers signal, carries out the attitude algorithm in next moment, cycle calculations.
Priority Applications (1)
Application Number  Priority Date  Filing Date  Title 

CN200810076417.0A CN106342284B (en)  20080818  20080818  A kind of flight carrier attitude is determined method 
Applications Claiming Priority (1)
Application Number  Priority Date  Filing Date  Title 

CN200810076417.0A CN106342284B (en)  20080818  20080818  A kind of flight carrier attitude is determined method 
Publications (1)
Publication Number  Publication Date 

CN106342284B true CN106342284B (en)  20111123 
Family
ID=57797860
Family Applications (1)
Application Number  Title  Priority Date  Filing Date 

CN200810076417.0A CN106342284B (en)  20080818  20080818  A kind of flight carrier attitude is determined method 
Country Status (1)
Country  Link 

CN (1)  CN106342284B (en) 
Cited By (17)
Publication number  Priority date  Publication date  Assignee  Title 

CN106979779A (en) *  20170522  20170725  深圳市靖洲科技有限公司  A kind of unmanned vehicle realtime attitude measuring method 
CN107063173A (en) *  20170613  20170818  广州辛群科技有限公司  Angle detecting method and joint motions angle detecting system 
CN107117280A (en) *  20170511  20170901  南方科技大学  Dirigible landing control method and device 
CN107131865A (en) *  20170613  20170905  广州辛群科技有限公司  Angle detection device 
CN107272718A (en) *  20170619  20171020  歌尔科技有限公司  Attitude control method and device based on Kalman filtering 
CN107478223A (en) *  20160608  20171215  南京理工大学  A kind of human body attitude calculation method based on quaternary number and Kalman filtering 
CN107817821A (en) *  20171027  20180320  成都鼎信精控科技有限公司  A kind of stable head and control method based on MEMS gyroscope combination 
CN108318038A (en) *  20180126  20180724  南京航空航天大学  A kind of quaternary number Gaussian particle filtering pose of mobile robot calculation method 
CN108475066A (en) *  20170421  20180831  深圳市大疆创新科技有限公司  Unmanned vehicle computation method for attitude, flight controller and unmanned vehicle 
CN108805175A (en) *  20180521  20181113  郑州大学  A kind of flight attitude clustering method of aircraft and analysis system 
CN108955671A (en) *  20180425  20181207  珠海全志科技股份有限公司  A kind of Kalman filtering air navigation aid based on magnetic declination, magnetic dip angle 
CN109030867A (en) *  20170608  20181218  海智芯株式会社  The method and apparatus for calculating angular speed using acceleration transducer and geomagnetic sensor 
CN109163721A (en) *  20180918  20190108  河北美泰电子科技有限公司  Attitude measurement method and terminal device 
CN109883444A (en) *  20190225  20190614  航天科工防御技术研究试验中心  A kind of attitude angle coupling error compensation method, device and electronic equipment 
CN110440797A (en) *  20190828  20191112  广州小鹏汽车科技有限公司  Vehicle attitude estimation method and system 
CN110887480A (en) *  20191211  20200317  中国空气动力研究与发展中心低速空气动力研究所  Flight attitude estimation method and system based on MEMS sensor 
WO2021031974A1 (en) *  20190819  20210225  深圳市道通智能航空技术有限公司  Method for selecting initial value of course angle of unmanned aerial vehicle and unmanned aerial vehicle 

2008
 20080818 CN CN200810076417.0A patent/CN106342284B/en not_active IP Right Cessation
Cited By (19)
Publication number  Priority date  Publication date  Assignee  Title 

CN107478223A (en) *  20160608  20171215  南京理工大学  A kind of human body attitude calculation method based on quaternary number and Kalman filtering 
CN108475066A (en) *  20170421  20180831  深圳市大疆创新科技有限公司  Unmanned vehicle computation method for attitude, flight controller and unmanned vehicle 
CN108475066B (en) *  20170421  20210219  深圳市大疆创新科技有限公司  Unmanned aerial vehicle attitude calculation method, flight controller and unmanned aerial vehicle 
CN107117280A (en) *  20170511  20170901  南方科技大学  Dirigible landing control method and device 
CN106979779A (en) *  20170522  20170725  深圳市靖洲科技有限公司  A kind of unmanned vehicle realtime attitude measuring method 
CN109030867A (en) *  20170608  20181218  海智芯株式会社  The method and apparatus for calculating angular speed using acceleration transducer and geomagnetic sensor 
CN107131865A (en) *  20170613  20170905  广州辛群科技有限公司  Angle detection device 
CN107063173A (en) *  20170613  20170818  广州辛群科技有限公司  Angle detecting method and joint motions angle detecting system 
CN107272718B (en) *  20170619  20200724  歌尔科技有限公司  Attitude control method and device based on Kalman filtering 
CN107272718A (en) *  20170619  20171020  歌尔科技有限公司  Attitude control method and device based on Kalman filtering 
CN107817821A (en) *  20171027  20180320  成都鼎信精控科技有限公司  A kind of stable head and control method based on MEMS gyroscope combination 
CN108318038A (en) *  20180126  20180724  南京航空航天大学  A kind of quaternary number Gaussian particle filtering pose of mobile robot calculation method 
CN108955671A (en) *  20180425  20181207  珠海全志科技股份有限公司  A kind of Kalman filtering air navigation aid based on magnetic declination, magnetic dip angle 
CN108805175A (en) *  20180521  20181113  郑州大学  A kind of flight attitude clustering method of aircraft and analysis system 
CN109163721A (en) *  20180918  20190108  河北美泰电子科技有限公司  Attitude measurement method and terminal device 
CN109883444A (en) *  20190225  20190614  航天科工防御技术研究试验中心  A kind of attitude angle coupling error compensation method, device and electronic equipment 
WO2021031974A1 (en) *  20190819  20210225  深圳市道通智能航空技术有限公司  Method for selecting initial value of course angle of unmanned aerial vehicle and unmanned aerial vehicle 
CN110440797A (en) *  20190828  20191112  广州小鹏汽车科技有限公司  Vehicle attitude estimation method and system 
CN110887480A (en) *  20191211  20200317  中国空气动力研究与发展中心低速空气动力研究所  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 inflight coarse alignment  
CN103245360B (en)  Carrierborne 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)  Selfcalibrated 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)  Microdynamic 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 selfconfirming method based on fullfunction suncompass  
CN101726295B (en)  Unscented Kalman filterbased method for tracking inertial pose according to acceleration compensation  
CN101514899B (en)  Optical fibre gyro strapdown inertial navigation system error inhibiting method based on singleshaft rotation  
Zhou et al.  Integrated navigation system for a lowcost quadrotor aerial vehicle in the presence of rotor influences  
CN101706281B (en)  Inertia/astronomy/satellite highprecision integrated navigation system and navigation method thereof  
Huang et al.  Kalmanfilteringbased inmotion coarse alignment for odometeraided SINS  
CN102692225B (en)  Attitude heading reference system for lowcost small unmanned aerial vehicle  
CN103776446B (en)  A kind of pedestrian's independent navigation computation based on double MEMSIMU  
CN102980577B (en)  Microstrapdown altitude heading reference system and working method thereof  
CN100356139C (en)  Miniature assembled gesture measuring system for minisatellite  
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 nonpayment of annual fee 
Granted publication date: 20111123 Termination date: 20180818 

CF01  Termination of patent right due to nonpayment of annual fee 