CN109725649A - One kind determining high algorithm based on barometer/IMU/GPS Multi-sensor Fusion rotor wing unmanned aerial vehicle - Google Patents
One kind determining high algorithm based on barometer/IMU/GPS Multi-sensor Fusion rotor wing unmanned aerial vehicle Download PDFInfo
- Publication number
- CN109725649A CN109725649A CN201811632978.4A CN201811632978A CN109725649A CN 109725649 A CN109725649 A CN 109725649A CN 201811632978 A CN201811632978 A CN 201811632978A CN 109725649 A CN109725649 A CN 109725649A
- Authority
- CN
- China
- Prior art keywords
- gps
- barometer
- height
- formula
- measurement
- Prior art date
- Legal status (The legal status 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 status listed.)
- Pending
Links
Landscapes
- Position Fixing By Use Of Radio Waves (AREA)
- Navigation (AREA)
Abstract
The present invention provides one kind to determine high algorithm based on barometer/IMU/GPS Multi-sensor Fusion rotor wing unmanned aerial vehicle, comprising the following steps: step 1, carries out pretreatment to barometer and carries out height modeling to barometrical measurement height；Step 2, the vertical height x under geographic coordinate system is takenz, vertical velocity vz, accelerometer Z axis zero bias bzAnd barometer zero bias bpThe state equation of Kalman filtering algorithm is constructed as quantity of state X；Step 3, the measurement equation of barometer height and the measurement equation of GPS velocity are constructed according to status predication equation；Step 4, in Kalman filtering algorithm quantity of state X and error covariance matrix be updated；Step 5, it when barometrical data update, barometer height is measured more to newly arrive obtains the i.e. vertical height of barometrical height, and when GPS velocity updates, GPS velocity is measured more to newly arrive obtain GPS velocity i.e. vertical velocity.
Description
Technical field
The invention belongs to unmanned planes to determine high field, and in particular to one kind is based on barometer/IMU/GPS Multi-sensor Fusion
Rotor wing unmanned aerial vehicle determines high algorithm.
Background technique
Rotor class unmanned plane often carries out fixed high control, guarantees unmanned plane height in outdoor carry out high-speed flight
Stability.The sensor that airborne end can provide elevation information mainly has barometer, GPS, ultrasonic wave etc..Wherein, the height of GPS is surveyed
Error is measured usually between 5-10m, is not used to the height control of unmanned plane；Ultrasonic wave is provided apart from the relatively high of ground
Degree, and measuring range is limited；Usual business unmanned plane flies control and generally obtains drone flying height using barometer, carries out height
Control.
The relationship of atmospheric pressure and height above sea level can be indicated by following formula.By barometric surveying barometric information, by changing
Calculate the height above sea level of you can get it current location point.However in unmanned plane high-speed flight, often made to bring about the desired sensation by air-flow, gust influence
Pressure meter measurement deviates true atmospheric pressure value.When Fig. 1 is that unmanned plane does high airline operation surely, the unmanned plane during flying speed of airborne SD card record
The relation curve for the height that degree and barometer converse.Alt is unmanned plane altitude datum in Fig. 1, and BarAlt is barometer reckoning
Height out, Spd are the flying speed of unmanned plane, as shown in Figure 1, accelerating and slowing down and high-speed flight whenever unmanned plane is done
When, barometrical altitude information will be generated 2~5 meters of height error by airflow influence.If carrying out fixed height according to this altitude information
Control, necessarily causes unmanned plane is born accident occurs.
Some Fei Kong producers attempt in unmanned plane high-speed flight, maintain height by adjusting the power output of unmanned plane
Constant, this is not obviously solved the problems, such as inherently, is also had and is carried out fixed height using ultrasonic wave or millimetre-wave radar, but this is provided
Be all relative altitude and influenced by complex environment below unmanned plane.In some higher-end businesses using inner, be often used RTK carry out it is high
Degree control, but its cost is very high, and use condition is complex.
Summary of the invention
The present invention is to carry out to solve the above-mentioned problems, and it is an object of the present invention to provide a kind of more based on barometer/IMU/GPS
The rotor wing unmanned aerial vehicle of sensor fusion determines high algorithm.
The present invention provides one kind to determine high algorithm based on barometer/IMU/GPS Multi-sensor Fusion rotor wing unmanned aerial vehicle, has
There is such feature, comprising the following steps: step 1, pretreatment is carried out to barometer and is carried out to barometrical measurement height high
Degree modeling；
Step 2, the vertical height x under geographic coordinate system is takenz, vertical velocity vz, accelerometer Z axis zero bias bzAnd air pressure
Count zero bias bpThe state equation of Kalman filtering algorithm is constructed as quantity of state X；
Step 3, the measurement equation of barometer height and the measurement equation of GPS velocity are constructed according to status predication equation；
Step 4, in Kalman filtering algorithm quantity of state X and error covariance matrix be updated；
Step 5, it when barometrical data update, barometer height is measured more to newly arrive obtains barometrical height
I.e. vertical height, and when GPS velocity updates, it GPS velocity is measured more to newly arrive obtains GPS velocity i.e. vertical velocity.
High algorithm is determined based on barometer/IMU/GPS Multi-sensor Fusion rotor wing unmanned aerial vehicle in one kind provided by the invention
In, it can also have the following features: wherein, in step 2, status predication equation are as follows:
In formula (1), X (t) is the quantity of state of t moment,For the predicted state amount at t+1 moment, F (t) turns for state
Matrix is moved,B (t) is control matrix,U (t) be control amount, U (t)=naz,
InnazFor the vertical acceleration under geographic coordinate system, it is calculated by the aviation attitude system and accelerometer data of unmanned plane, W
(t)=[wpz wvz wbz wbp]T, W (t) is zero-mean white noise and meets W~N (0, Q), and Q is system noise matrix.
High algorithm is determined based on barometer/IMU/GPS Multi-sensor Fusion rotor wing unmanned aerial vehicle in one kind provided by the invention
In, it can also have the following features: wherein, in step 3, the measurement equation of barometer height are as follows:
Zbaro(t)=Hbaro(t)X(t)+vbaro(t) (2)
The measurement equation of GPS velocity are as follows:
Zgps(t)=Hgps(t)X(t)+vgps(t) (3)
In formula (2), HbaroFor barometrical measurement matrix, Hbaro=[1 00 1], ZbaroIt (t) is the air pressure of t moment
The height of meter, vbaro(t) it is zero-mean white noise and meets vbaro(t)~N (0, Vb), VbFor barometrical measurement noise matrix,
In formula (3), HgpsFor the measurement matrix of GPS velocity, Hgps=[0 10 0], Zgps(t) it is measured for t moment GPS
Vertical velocity, vgpsFor zero-mean white noise and meet vgps~N (0, Vg), VgFor the noise matrix of vertical velocity.
High algorithm is determined based on barometer/IMU/GPS Multi-sensor Fusion rotor wing unmanned aerial vehicle in one kind provided by the invention
In, it can also have the following features: wherein, the quantity of state X and error covariance in step 4, in Kalman filtering algorithm
Battle array renewal equation are as follows:
In formula (4) and formula (5),For the predicted state amount at k moment,For in formula (1)Xk-1
For the quantity of state at k-1 moment, Xk-1For the X (t), Φ in formula (1)k|k-1It is the B in formula (1) for the F (t) in formula (1), B
(t), UkFor the U (t) in formula (1),For the predicting covariance battle array at k moment, Pk-1For the error covariance at k-1 moment
Battle array, R UkNoise matrix, QkFor Q, i.e. system noise matrix.
High algorithm is determined based on barometer/IMU/GPS Multi-sensor Fusion rotor wing unmanned aerial vehicle in one kind provided by the invention
In, it can also have the following features: wherein, in step 5, the measurement renewal equation of barometer height are as follows:
The measurement renewal equation of GPS velocity are as follows:
In formula (6), formula (7) and formula (8), KkFor kalman gain coefficient, HkFor the H in formula (2)baro, VbFor
The corresponding sensor of barometer measures noise matrix, and I is unit matrix, PkFor updated error covariance matrix, XkAfter updating
The k moment quantity of state, pass through quantity of state XkThe middle vertical height x for obtaining the k momentzWith barometer zero bias bp,When for k
The predicted state amount at quarter, ZbaroFor the Z in barometrical height, that is, formula (2)baro(t), Hbaro=[1 00 1],
In formula (9), formula (10) and formula (11), KkFor kalman gain coefficient, HkIt is H in formula (3)gps, VgFor
The noise matrix of vertical velocity, I are unit matrix, PkFor updated error covariance matrix, XkFor updated quantity of state, lead to
Cross quantity of state XkTo obtain the vertical velocity v at k momentz,For the quantity of state at k moment, ZgpsFor vertical velocity, that is, formula (3)
In Zgps(t), Hgps=[0 10 0].
High algorithm is determined based on barometer/IMU/GPS Multi-sensor Fusion rotor wing unmanned aerial vehicle in one kind provided by the invention
In, it can also have the following features: wherein, further include following sub-step in step 1:
Step 1-1 prevents air-flow in short-term and the illumination from interfering barometer in barometer upper press cover sponge；
Step 1-2 carries out slipping smoothness filtering to height is measured, so that the influence for measuring noise to height is measured is reduced,
Measure the calculation formula of height are as follows:
H=h0+εb+ w,
Wherein, h is the vertical height x under barometrical measurement height as geographic coordinate systemz, h0For current true ideal
Highly, εbFor barometrical constant value drift noise, being caused by temperature factor, humidity factor and climatic factor, w is white noise,
Caused by system quantifies noise.
High algorithm is determined based on barometer/IMU/GPS Multi-sensor Fusion rotor wing unmanned aerial vehicle in one kind provided by the invention
In, it can also have the following features: wherein, barometrical measurement noise matrix VbThe acceleration of motion of unmanned plane is big, nothing
Increased when man-machine rotational angular velocity is greatly and the flying speed of unmanned plane is fast to reduce barometer height to Kalman and filter
The amendment dynamics of altitude information in wave device, passes through the three axis accelerometer a of IMUx,ay,azCome to the acceleration of motion of unmanned plane into
Row judgement, whenWhen, ε is acceleration of motion judgment threshold, increases the corresponding measurement noise of barometer
Matrix Vb, height h is measured to the amendment dynamics of filter altitude information to reduce, to complete to correspond to barometrical measurement more
Newly, pass through the three axis accelerometer g of IMUx,gy,gzCome judge unmanned plane in the rotational angular velocity of X-axis or Y-axis, whenWhen, β is
Rotational angular velocity judgment threshold increases the corresponding measurement noise matrix V of barometerb, height h is measured to filter height to reduce
The amendment dynamics of data when unmanned plane high-speed flight, that is, is worked as to complete to correspond to barometrical measurement update | Vh| > vγ
When, VhFor the horizontal velocity of unmanned plane, vγFor velocity estimated threshold value, increase the corresponding measurement noise matrix V of barometerb, to reduce
Height h is measured to the amendment dynamics of filter altitude information, to complete to correspond to barometrical measurement update.
The action and effect of invention
It is a kind of based on the fixed height of barometer/IMU/GPS Multi-sensor Fusion rotor wing unmanned aerial vehicle involved according to the present invention
Algorithm, because can be when unmanned plane carries out strong motor-driven and high-speed flight, by adjusting the measurement in Kalman filtering algorithm
Noise is guaranteed to reduce influence of the barometer disturbance for filter height by the vertical velocity of fusion GPS measurement
Vertical velocity precision of the Kalman filtering algorithm in weak measurement amendment dynamics, so can be improved Kalman filtering algorithm
For the stability of unmanned plane height output；Because being calculated by the barometrical data fusion of GPS/IMU/ and using Kalman filtering
Method calculates the vertical height of unmanned plane with speed, so, it is not needing to introduce such as RTK, millimetre-wave radar, ultrasonic distance measurement
The additional sensors such as module, it will be able to it realizes height-lock control of the rotor wing unmanned aerial vehicle under high maneuvering condition, ensure that flight safety,
Improve flight operation quality.Therefore one kind of the invention be based on barometer/IMU/GPS Multi-sensor Fusion rotor nobody
The fixed high algorithmic method of machine is easy, and data processing is quick, can provide surely in the case where not adding additional sensors for unmanned plane
Fixed reliable altitude information.
Detailed description of the invention
Fig. 1 is that the unmanned plane in background technique of the invention determines the flying speed of high airline operation and the pass of barometer height
It is curve graph；
Barometer/IMU/GPS data fusion flow chart when Fig. 2 is unmanned plane height-lock control in the embodiment of the present invention；
Fig. 3 is the flight path figure of unmanned plane in the embodiment of the present invention；
Fig. 4 is a kind of fixed based on barometer/IMU/GPS Multi-sensor Fusion rotor wing unmanned aerial vehicle in the embodiment of the present invention
The height and RTK height measured data contrast curve chart that high algorithm is calculated.
Specific embodiment
In order to which the technological means for realizing the present invention is easy to understand with effect, with reference to embodiments and attached drawing is to this
Invention is specifically addressed.
Embodiment:
One kind of the present embodiment is based on barometer/IMU/GPS Multi-sensor Fusion rotor wing unmanned aerial vehicle and determines high algorithm, including
Following steps:
Step 1, pretreatment is carried out to barometer and height modeling is carried out to barometrical measurement height.
Further include following sub-step in step 1: step 1-1 prevents air-flow in short-term and light in barometer upper press cover sponge
It is interfered according to barometer.
Step 1-2 carries out slipping smoothness filtering to height is measured, so that the influence for measuring noise to height is measured is reduced,
Measure the calculation formula of height are as follows:
H=h0+εb+ w,
Wherein, h is the vertical height x under barometrical measurement height as geographic coordinate systemz, h0For current true ideal
Highly, εbFor barometrical constant value drift noise, being caused by temperature factor, humidity factor and climatic factor, w is white noise,
Caused by system quantifies noise.
Barometer/IMU/GPS data fusion flow chart when Fig. 2 is unmanned plane height-lock control in the embodiment of the present invention.
As shown in Fig. 2, step 2, takes the vertical height x under geographic coordinate systemz, vertical velocity vz, accelerometer Z axis zero bias
bzAnd barometer zero bias bpThe state equation of Kalman filtering algorithm is constructed as quantity of state X.
In step 2, state equation are as follows:
In formula (1), X (t) is the quantity of state of t moment,For the predicted state amount at t+1 moment, F (t) turns for state
Matrix is moved,B (t) is control matrix,U (t) be control amount, U (t)=naz,
InnazFor the vertical acceleration under geographic coordinate system, it is calculated by the aviation attitude system and accelerometer data of unmanned plane, W
(t)=[wpz wvz wbz wbp]T, W (t) is zero-mean white noise and meets W~N (0, Q), and Q is system noise matrix.
Step 3, the measurement equation of barometer height and the measurement equation of GPS velocity are constructed according to status predication equation.
In step 3, the measurement equation of barometer height are as follows:
Zbaro(t)=Hbaro(t)X(t)+vbaro(t) (2)
The measurement equation of GPS velocity are as follows:
Zgps(t)=Hgps(t)X(t)+vgps(t) (3)
In formula (2), HbaroFor barometrical measurement matrix, Hbaro=[1 00 1], ZbaroIt (t) is the air pressure of t moment
The height of meter, vbaro(t) it is zero-mean white noise and meets vbaro(t)~N (0, Vb), VbFor barometrical measurement noise matrix,
In formula (3), HgpsFor the measurement matrix of GPS velocity, Hgps=[0 10 0], Zgps(t) it is measured for t moment GPS
Vertical velocity, vgpsFor zero-mean white noise and meet vgps~N (0, Vg), VgFor the noise matrix of vertical velocity.
Barometrical measurement noise matrix VbThe acceleration of motion of unmanned plane is big, unmanned plane rotational angular velocity is big and
Increased when the flying speed of unmanned plane is fast to reduce barometer height to the correcting force of altitude information in Kalman filter
Degree,
Pass through the three axis accelerometer a of IMUx,ay,azThe acceleration of motion of unmanned plane judged, whenWhen, ε is acceleration of motion judgment threshold, increases the corresponding measurement noise matrix V of barometerb, come
It reduces and measures height h to the amendment dynamics of filter altitude information, thus complete to correspond to barometrical measurement update,
Pass through the three axis accelerometer g of IMUx,gy,gzCome judge unmanned plane in the rotational angular velocity of X-axis or Y-axis, when
When, β is rotational angular velocity judgment threshold, increases the corresponding measurement noise matrix V of barometerb, height h is measured to filtering to reduce
The amendment dynamics of device altitude information, thus complete to correspond to barometrical measurement update,
When unmanned plane high-speed flight, that is, work as | Vh| > vγWhen, VhFor the horizontal velocity of unmanned plane, vγFor velocity estimated threshold
Value increases the corresponding measurement noise matrix V of barometerb, height h is measured to the amendment dynamics of filter altitude information to reduce,
It is updated to complete to correspond to barometrical measurement.
Step 4, in Kalman filtering algorithm quantity of state X and error covariance matrix be updated.
Quantity of state X and error covariance matrix renewal equation in step 4, in Kalman filtering algorithm are as follows:
In formula (4) and formula (5),For the predicted state amount at k moment,For in formula (1)Xk-1For
The quantity of state at k-1 moment, Xk-1For the X (t), Φ in formula (1)k|k-1It is the B in formula (1) for the F (t) in formula (1), B
(t), UkFor the U (t) in formula (1),For the predicting covariance battle array at k moment, Pk-1For the error covariance at k-1 moment
Battle array, R UkNoise matrix, QkFor Q, i.e. system noise matrix.
Step 5, it when barometrical data update, barometer height is measured more to newly arrive obtains barometrical height
I.e. vertical height, and when GPS velocity updates, it GPS velocity is measured more to newly arrive obtains GPS velocity i.e. vertical velocity,
Before the measurement update for carrying out GPS velocity, GPS signal is measured by indexs such as the visible star number of GPS and vertical dilution of precisions,
And starts GPS velocity in the good situation of GPS signal and measure update.
The measurement renewal equation of barometer height are as follows:
The measurement renewal equation of GPS velocity are as follows:
In formula (6), formula (7) and formula (8), KkFor kalman gain coefficient, HkFor the H in formula (2)baro, VbFor
The corresponding sensor of barometer measures noise matrix, and I is unit matrix, PkFor updated error covariance matrix, XkAfter updating
The k moment quantity of state, pass through quantity of state XkThe middle vertical height x for obtaining the k momentzWith barometer zero bias bp,For the k moment
Predicted state amount, ZbaroFor the Z in barometrical height, that is, formula (2)baro(t), Hbaro=[1 00 1],
In formula (9), formula (10) and formula (11), KkFor kalman gain coefficient, HkIt is H in formula (3)gps, VgFor
The noise matrix of vertical velocity, I are unit matrix, PkFor updated error covariance matrix, XkFor updated quantity of state, lead to
Cross quantity of state XkTo obtain the vertical velocity v at k momentz,For the quantity of state at k moment, ZgpsFor vertical velocity, that is, formula (3)
In Zgps(t), Hgps=[0 10 0].
Barometer/IMU/GPS Multi-sensor Fusion rotor wing unmanned aerial vehicle is based on using one kind of the present embodiment and determines high algorithm
Carry out the fixed high measure of merit process of unmanned plane:
The present embodiment is determined high algorithm based on barometer/IMU/GPS Multi-sensor Fusion rotor wing unmanned aerial vehicle and increasing income
Fly to program on control hardware platform Pixhawk2 and realize, hardware composition specifically includes that CPU:STM32F427
IMU:MPU9250
Barometer: MS5611
GPS:ublox-M8N
For the ease of quantitative analysis altitude information, it is equipped with RTK high-accuracy position system Novatel OEM615 board,
The board highest supports the RTK of 50HZ to position output, and positional accuracy measurement is better than 3cm.
Fig. 3 is the flight path figure of unmanned plane in the embodiment of the present invention.
As shown in figure 3, unmanned plane records flight number by airborne SD card according to flight line, and in flight course in real time
According to by matched earth station MissionPlaner progress data analysis, taking off after RTK fixed solution, it is winged to carry out surely high course line
Row, and the present embodiment one kind is determined into high algorithm based on barometer/IMU/GPS Multi-sensor Fusion rotor wing unmanned aerial vehicle and is calculated
Height and RTK height compare.
Fig. 4 is a kind of fixed based on barometer/IMU/GPS Multi-sensor Fusion rotor wing unmanned aerial vehicle in the embodiment of the present invention
The height and RTK height measured data contrast curve chart that high algorithm is calculated.
As shown in figure 4, one kind of the present embodiment is melted based on barometer/IMU/GPS multisensor in entire flight course
The rotor wing unmanned aerial vehicle of conjunction is determined the height that high algorithm is calculated and is differed with RTK altitude datum no more than 0.5m, and obtained height
Data smoothing is stablized.
In summary, one kind of the present embodiment is based on the fixed height of barometer/IMU/GPS Multi-sensor Fusion rotor wing unmanned aerial vehicle
Algorithm can effectively ensure that the stability that height is calculated.
The action and effect of embodiment
One kind according to involved in the present embodiment is fixed based on barometer/IMU/GPS Multi-sensor Fusion rotor wing unmanned aerial vehicle
High algorithm, because can be when unmanned plane carries out strong motor-driven and high-speed flight, by adjusting the amount in Kalman filtering algorithm
Noise is surveyed to reduce influence of the barometer disturbance for filter height, and protect by the vertical velocity of fusion GPS measurement
Vertical velocity precision of the Kalman filtering algorithm in weak measurement amendment dynamics is demonstrate,proved, so can be improved Kalman filtering calculation
The stability that method exports unmanned plane height；Because passing through the barometrical data fusion of GPS/IMU/ and utilizing Kalman filtering
Algorithm calculates the vertical height of unmanned plane with speed, so, it is not needing to introduce such as RTK, millimetre-wave radar, ultrasound
Away from additional sensors such as modules, it will be able to realize height-lock control of the rotor wing unmanned aerial vehicle under high maneuvering condition, ensure that flight peace
Entirely, flight operation quality is improved.Therefore one kind of the present embodiment is based on the rotation of barometer/IMU/GPS Multi-sensor Fusion
The fixed high algorithmic method of wing unmanned plane is easy, and data processing is quick, can be unmanned plane in the case where not adding additional sensors
Reliable and stable altitude information is provided.
Above embodiment is preferred case of the invention, the protection scope being not intended to limit the invention.
Claims (7)
1. one kind determines high algorithm based on barometer/IMU/GPS Multi-sensor Fusion rotor wing unmanned aerial vehicle, which is characterized in that including
Following steps:
Step 1, pretreatment is carried out to barometer and height modeling is carried out to the barometrical measurement height；
Step 2, the vertical height x under geographic coordinate system is takenz, vertical velocity vz, accelerometer Z axis zero bias bzAnd barometer zero
Inclined bpThe status predication equation of Kalman filtering algorithm is constructed as quantity of state X；
Step 3, the measurement equation of the barometer height and the measurement side of GPS velocity are constructed according to the status predication equation
Journey；
Step 4, in the Kalman filtering algorithm quantity of state X and error covariance matrix be updated；
Step 5, it when the barometrical data update, the barometer height is measured more to newly arrive obtains the air pressure
The height of meter, that is, vertical height, and when the GPS velocity updates, measures GPS velocity more to newly arrive and obtains GPS speed
Degree is vertical velocity.
2. according to claim 1 a kind of based on the fixed high calculation of barometer/IMU/GPS Multi-sensor Fusion rotor wing unmanned aerial vehicle
Method, it is characterised in that:
Wherein, in the step 2, status predication equation are as follows:
In formula (1), X (t) is the quantity of state of t moment,For the predicted state amount at t+1 moment, F (t) is that state shifts square
Battle array,B (t) is control matrix,U (t) be control amount, U (t)=naz, whereinnaz
For the vertical acceleration under geographic coordinate system, it is calculated by the aviation attitude system and accelerometer data of unmanned plane, W (t)=
[wpz wvz wbz wbp]T, W (t) is zero-mean white noise and meets W~N (0, Q), and Q is system noise matrix.
3. according to claim 1 a kind of based on the fixed high calculation of barometer/IMU/GPS Multi-sensor Fusion rotor wing unmanned aerial vehicle
Method, it is characterised in that:
Wherein, in the step 3, the measurement equation of the barometer height are as follows:
Zbaro(t)=Hbaro(t)X(t)+vbaro(t) (2)
The measurement equation of the GPS velocity are as follows:
Zgps(t)=Hgps(t)X(t)+vgps(t) (3)
In formula (2), HbaroFor the barometrical measurement matrix, Hbaro=[1 00 1], ZbaroIt (t) is the described of t moment
Barometrical height, vbaro(t) it is zero-mean white noise and meets vbaro(t)~N (0, Vb), VbFor the barometrical measurement
Noise matrix,
In formula (3), HgpsFor the measurement matrix of the GPS velocity, Hgps=[0 10 0], Zgps(t) it is measured for t moment GPS
Vertical velocity, vgpsFor zero-mean white noise and meet vgps~N (0, Vg), VgFor the noise matrix of the vertical velocity.
4. according to claim 1 a kind of based on the fixed high calculation of barometer/IMU/GPS Multi-sensor Fusion rotor wing unmanned aerial vehicle
Method, it is characterised in that:
Wherein, quantity of state X and error covariance matrix renewal equation in the step 4, in the Kalman filtering algorithm are as follows:
In formula (4) and formula (5),For the predicted state amount at k moment,For in formula (1)Xk-1For k-1
The quantity of state at moment, Xk-1For the X (t), Φ in formula (1)kk-1It is the B (t), U in formula (1) for the F (t) in formula (1), Bk
For the U (t) in formula (1),For the predicting covariance battle array at k moment, Pk-1For the error covariance matrix at k-1 moment, R is
UkNoise matrix, QkFor Q, i.e. system noise matrix.
5. according to claim 1 a kind of based on the fixed high calculation of barometer/IMU/GPS Multi-sensor Fusion rotor wing unmanned aerial vehicle
Method, it is characterised in that:
Wherein, in the step 5, the measurement renewal equation of the barometer height are as follows:
The measurement renewal equation of the GPS velocity are as follows:
In formula (6), formula (7) and formula (8), KkFor kalman gain coefficient, HkFor the H in formula (2)baro, VbIt is described
The corresponding sensor of barometer measures noise matrix, and I is unit matrix, PkFor updated error covariance matrix, XkAfter updating
The k moment quantity of state, pass through quantity of state XkThe middle vertical height x for obtaining the k momentzWith barometer zero bias bp,When for k
The predicted state amount at quarter, ZbaroFor the Z in the barometrical height, that is, formula (2)baro(t), Hbaro=[1 00 1],
In formula (9), formula (10) and formula (11), KkFor kalman gain coefficient, HkIt is H in formula (3)gps, VgIt is described
The noise matrix of vertical velocity, I are unit matrix, PkFor updated error covariance matrix, XkFor updated quantity of state, lead to
Cross quantity of state XkTo obtain the vertical velocity v at k momentz,For the quantity of state at k moment, ZgpsFor vertical velocity, that is, formula (3)
In Zgps(t), Hgps=[0 10 0].
6. according to claim 1 a kind of based on the fixed high calculation of barometer/IMU/GPS Multi-sensor Fusion rotor wing unmanned aerial vehicle
Method, it is characterised in that:
Wherein, further include following sub-step in the step 1:
Step 1-1 prevents air-flow in short-term and illumination from interfering the barometer in the barometer upper press cover sponge；
Step 1-2, carries out slipping smoothness filtering to the measurement height, measures noise to the shadow for measuring height to reduce
It rings, the calculation formula for measuring height are as follows:
H=h0+εb+ w,
Wherein, h is the vertical height x under the barometrical measurement height as geographic coordinate systemz, h0It is current true
Ideal height, εbFor the barometrical constant value drift noise, caused by temperature factor, humidity factor and climatic factor, w is
White noise is caused by system quantifies noise.
7. according to claim 1 a kind of based on the fixed high calculation of barometer/IMU/GPS Multi-sensor Fusion rotor wing unmanned aerial vehicle
Method, it is characterised in that:
Wherein, the barometrical measurement noise matrix VbThe acceleration of motion of unmanned plane is big, the rotational angular velocity of unmanned plane is big
And unmanned plane flying speed it is fast when increased reduce the barometer height in the Kalman filter height
The amendment dynamics of data,
Pass through the three axis accelerometer a of IMUx,ay,azThe acceleration of motion of unmanned plane judged, when
When, ε is acceleration of motion judgment threshold, increases the corresponding measurement noise matrix V of the barometerb, high to reduce the measurement
H is spent to the amendment dynamics of filter altitude information, thus complete to correspond to the barometrical measurement update,
Pass through the three axis accelerometer g of IMUx,gy,gzCome judge unmanned plane in the rotational angular velocity of X-axis or Y-axis, whenWhen, β is
Rotational angular velocity judgment threshold increases the corresponding measurement noise matrix V of the barometerb, to reduce the measurement height h to filter
The amendment dynamics of wave device altitude information, thus complete to correspond to the barometrical measurement update,
When unmanned plane high-speed flight, that is, work as | Vh| > vγWhen, VhFor the horizontal velocity of the unmanned plane, vγFor velocity estimated threshold
Value, increases the corresponding measurement noise matrix V of the barometerb, filter altitude information is repaired to reduce the measurement height h
Positive dynamics, to complete to correspond to the barometrical measurement update.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811632978.4A CN109725649A (en) | 2018-12-29 | 2018-12-29 | One kind determining high algorithm based on barometer/IMU/GPS Multi-sensor Fusion rotor wing unmanned aerial vehicle |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811632978.4A CN109725649A (en) | 2018-12-29 | 2018-12-29 | One kind determining high algorithm based on barometer/IMU/GPS Multi-sensor Fusion rotor wing unmanned aerial vehicle |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN109725649A true CN109725649A (en) | 2019-05-07 |
Family
ID=66297585
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811632978.4A Pending CN109725649A (en) | 2018-12-29 | 2018-12-29 | One kind determining high algorithm based on barometer/IMU/GPS Multi-sensor Fusion rotor wing unmanned aerial vehicle |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109725649A (en) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111006659A (en) * | 2019-12-06 | 2020-04-14 | 江西洪都航空工业集团有限责任公司 | Navigation system with multi-navigation-source information fusion function |
| CN112629529A (en) * | 2020-12-15 | 2021-04-09 | 西安工业大学 | Indoor autonomous navigation method for unmanned aerial vehicle |
| CN113778120A (en) * | 2021-10-27 | 2021-12-10 | 北京航空航天大学 | Multi-sensor fusion unmanned aerial vehicle complex weather flight control method |
| CN115900646A (en) * | 2023-03-08 | 2023-04-04 | 北京云圣智能科技有限责任公司 | Height fusion navigation method and device, electronic equipment and storage medium |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20050251328A1 (en) * | 2004-04-05 | 2005-11-10 | Merwe Rudolph V D | Navigation system applications of sigma-point Kalman filters for nonlinear estimation and sensor fusion |
| CN101000244A (en) * | 2007-01-05 | 2007-07-18 | 北京航空航天大学 | High integral navigation device combined by MIMU/GPS/micromagnetic compass/barometric altimeter |
| CN104567799A (en) * | 2014-11-28 | 2015-04-29 | 天津大学 | Multi-sensor information fusion-based method for measuring height of small unmanned gyroplane |
| CN104808673A (en) * | 2015-02-12 | 2015-07-29 | 武汉顶翔智控科技有限公司 | Height estimation method for four-rotor aircraft based on Kalman filtering |
| CN105865453A (en) * | 2016-05-20 | 2016-08-17 | 南京航空航天大学 | Navigation system of position sensor and attitude sensor and fusion method of system |
| US9772186B1 (en) * | 2010-05-28 | 2017-09-26 | Tanenhaus & Associates, Inc. | Miniaturized inertial measurement and navigation sensor device and associated methods |
-
2018
- 2018-12-29 CN CN201811632978.4A patent/CN109725649A/en active Pending
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20050251328A1 (en) * | 2004-04-05 | 2005-11-10 | Merwe Rudolph V D | Navigation system applications of sigma-point Kalman filters for nonlinear estimation and sensor fusion |
| CN101000244A (en) * | 2007-01-05 | 2007-07-18 | 北京航空航天大学 | High integral navigation device combined by MIMU/GPS/micromagnetic compass/barometric altimeter |
| US9772186B1 (en) * | 2010-05-28 | 2017-09-26 | Tanenhaus & Associates, Inc. | Miniaturized inertial measurement and navigation sensor device and associated methods |
| CN104567799A (en) * | 2014-11-28 | 2015-04-29 | 天津大学 | Multi-sensor information fusion-based method for measuring height of small unmanned gyroplane |
| CN104808673A (en) * | 2015-02-12 | 2015-07-29 | 武汉顶翔智控科技有限公司 | Height estimation method for four-rotor aircraft based on Kalman filtering |
| CN105865453A (en) * | 2016-05-20 | 2016-08-17 | 南京航空航天大学 | Navigation system of position sensor and attitude sensor and fusion method of system |
Non-Patent Citations (1)
| Title |
|---|
| 闻泽阳 等: "基于MEMS IMU的室内导航系统设计", 《舰船电子工程》 * |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111006659A (en) * | 2019-12-06 | 2020-04-14 | 江西洪都航空工业集团有限责任公司 | Navigation system with multi-navigation-source information fusion function |
| CN112629529A (en) * | 2020-12-15 | 2021-04-09 | 西安工业大学 | Indoor autonomous navigation method for unmanned aerial vehicle |
| CN113778120A (en) * | 2021-10-27 | 2021-12-10 | 北京航空航天大学 | Multi-sensor fusion unmanned aerial vehicle complex weather flight control method |
| CN115900646A (en) * | 2023-03-08 | 2023-04-04 | 北京云圣智能科技有限责任公司 | Height fusion navigation method and device, electronic equipment and storage medium |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109725649A (en) | One kind determining high algorithm based on barometer/IMU/GPS Multi-sensor Fusion rotor wing unmanned aerial vehicle | |
| CN103472503B (en) | Sonde and upper air wind finding method based on INS | |
| Cho et al. | Wind estimation and airspeed calibration using a UAV with a single-antenna GPS receiver and pitot tube | |
| CN110631593A (en) | Multi-sensor fusion positioning method for automatic driving scene | |
| CN113124856B (en) | Visual inertia tight coupling odometer based on UWB (ultra wide band) online anchor point and metering method | |
| KR20150043392A (en) | Estimating a wind vector | |
| CN101858748A (en) | Fault-tolerance autonomous navigation method of multi-sensor of high-altitude long-endurance unmanned plane | |
| CN108592911B (en) | Four-rotor aircraft dynamic model/airborne sensor combined navigation method | |
| CN105242682A (en) | Target characteristic measurement system for drone aircraft | |
| CN106705936A (en) | Method and device for optimizing altitude of unmanned aerial vehicle | |
| CN105807087B (en) | It is determined as optimizing the method and apparatus of wind speed that the take-off weight of aircraft need to consider | |
| CN110286390A (en) | A kind of specified path wind measurement method, device and windfinding radar scaling method | |
| US20050040985A1 (en) | System and method for providing improved accuracy relative positioning from a lower end GPS receiver | |
| JP2010214979A (en) | Computer, landing route calculation program, and recording medium | |
| US10876920B1 (en) | Auxiliary aerial vehicles for flow characterization | |
| CN105738909A (en) | Unmanned helicopter work boundary extraction method suitable for low-altitude plant protection | |
| CN116992700B (en) | Method and equipment for determining navigation precision of logistics unmanned aerial vehicle | |
| CN105466423A (en) | Unmanned aerial vehicle navigation system and operation method thereof | |
| CN113671598A (en) | Combined high-altitude wind detection method | |
| US20200264209A1 (en) | System and method for determining wind direction and velocity measurement from altitude for an unmanned aerial vehicle | |
| CN115542363B (en) | Attitude measurement method suitable for vertical downward-looking aviation pod | |
| CN110388917B (en) | Aircraft monocular vision scale estimation method and device, aircraft navigation system and aircraft | |
| CN115727842A (en) | Rapid alignment method and system for unmanned aerial vehicle, computer equipment and storage medium | |
| CN114778887A (en) | Unmanned aerial vehicle wind measurement method and device based on improved triangular vector model | |
| KR102090615B1 (en) | Drone Control System Using Model Predictive Control |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| WD01 | Invention patent application deemed withdrawn after publication |
Application publication date: 20190507 |
|
| WD01 | Invention patent application deemed withdrawn after publication |