CN106767798A - A kind of position of Navigation of Pilotless Aircraft and the real-time estimation method of speed and system - Google Patents
A kind of position of Navigation of Pilotless Aircraft and the real-time estimation method of speed and system Download PDFInfo
- Publication number
- CN106767798A CN106767798A CN201611051078.1A CN201611051078A CN106767798A CN 106767798 A CN106767798 A CN 106767798A CN 201611051078 A CN201611051078 A CN 201611051078A CN 106767798 A CN106767798 A CN 106767798A
- Authority
- CN
- China
- Prior art keywords
- information
- speed
- estimation
- estimated
- acceleration
- 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.)
- Granted
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/18—Stabilised platforms, e.g. by gyroscope
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
Abstract
The present invention relates to a kind of position of Navigation of Pilotless Aircraft and the real-time estimation method of speed and system, method therein includes S1：The current reference position information of unmanned plane is obtained using externally measured unit；S2：The difference between current reference position information and speed displacement forecast model estimated location information is calculated, the site error between the reference position information and the speed displacement forecast model estimated location information is obtained；S3, designs third order pll wave filter；S4：Desin speed displacement prediction model；S5：The step S1 is returned, is circulated successively, realize the tracking to the speed and position of unmanned plane.The method principle is simple, easily realization and popularization, and optimal location and speed can be rapidly and accurately estimated from external location information.
Description
Technical field
The present invention relates to Navigation of Pilotless Aircraft technical field, more particularly to a kind of Navigation of Pilotless Aircraft position and the reality of speed
When method of estimation and system.
Background technology
The control system of either multi-rotor unmanned aerial vehicle or fixed-wing unmanned plane is required for providing in high precision and real-time
Position, speed and Attitude estimation, high-acruracy survey is the premise of high-precision control, and real-time has then corresponded to controlling cycle
Need.
Although satellite navigation receiver can provide error not with position, the velocity information that the time dissipates, data are defeated
The turnover rate for going out often well below the requirement of flight control system, and required for single-point locating system cannot also provide flight control system
Attitude information.Real-time position, speed and attitude information are provided, traditional method is namely based on Inertial Measurement Unit
The strap inertial navigation algorithm of (IMU, including 3 axle gyros, 3 axis accelerometers).Due to the requirement of low cost, Inertial Measurement Unit
Micromechanical gyro and micro-mechanical accelerometer are used mostly.Because the precision of device is poor, navigation error is caused to dissipate quickly.
In order to suppress the growth of error, the error of inertial navigation is corrected using the absolute method of measurement beyond IMU generally, such as 3
Axle magnetometer, barometertic altimeter, satellite navigation receiver is characterized in that error does not dissipate with the time.
Magnetic compass navigation algorithm based on 3 axle magnetometers can provide north orientation benchmark, suppress the course of inertial navigation system
Angle error.3-dimensional position and velocity information can be provided based on satellite navigation receiver, suppresses position, the speed of inertial navigation system
Degree and horizontal attitude angle error.Elevation information based on barometertic altimeter can make up satellite navigation receiver in elevation location side
The error in face.High-precision position, speed, Attitude estimation can be both provided by the inertial navigation system after error correction,
But also the need for meeting real-time.
Absolute measurements (such as magnetic compass, barometertic altimeter, the satellite navigation receiver not dissipated with the time using outside
Deng) method that suppresses INS errors is mainly and uses Kalman filter.By setting up state equation and observation side
Journey estimated the quantity of state in state equation, obtains filtered position, speed, attitude, gyro zero partially and acceleration
Meter zero partially etc..Typical method has the pine combination based on position, speed, based on pseudorange, tight integration of pseudorange rates etc..
But the operand of Kalman filter is bigger than normal, operand is directly proportional to the dimension square of wave filter, is unsuitable for
In flight control system based on single-chip microcomputer.And application under the conditions of nonwhite noise and the unreasonable of parameter setting are often led
Cause wave filter unstable.Therefore other modification methods beyond Kalman filter, such as modification method based on weight are occurred in that.
The maximum advantage of this method is exactly that compared with Kalman filter, amount of calculation is substantially relatively low.It is disadvantageous in that weight system
The corresponding physical significance of several settings is not obvious.
The content of the invention
Be overcome prior art exist above-mentioned technical problem, the invention provides a kind of Navigation of Pilotless Aircraft position and
The real-time estimation method of speed, its principle is simple, easily realization and popularization, rapidly and accurately can estimate from external location information
Go out optimal location and speed.
The technical scheme that the present invention solves above-mentioned technical problem is as follows：A kind of position of Navigation of Pilotless Aircraft and the reality of speed
When method of estimation, it includes：
S1, the reference position information of unmanned plane is obtained using externally measured unit；
S2, calculates the difference between reference position information and speed-displacement prediction model estimated location information, obtains described
Site error between reference position information and the speed-displacement prediction model estimated location information；
S3, designs third order pll wave filter；
Position between the reference position information and the speed-displacement prediction model estimated location information that will obtain
Error as the third order pll wave filter input, through the treatment of the third order pll wave filter, output estimation plus
The velocity information of velocity information and the previous moment of estimation；
S4：Desin speed-displacement prediction model；
Using the velocity information of the acceleration information of the estimation and the previous moment of the estimation as the speed-displacement
The input of forecast model, through the treatment of the speed-displacement prediction model, output current time speed-displacement prediction model is estimated
The positional information that the velocity information and speed of meter-displacement prediction model are estimated；
S5, returns to the step S1, circulates successively, realizes the tracking to the speed and position of unmanned plane.
The beneficial effects of the invention are as follows：Estimate unmanned plane using third order pll wave filter and speed-displacement prediction model
Speed and position, compared with Kalman filter, its operand is smaller, rapidly and accurately can estimate from external location information
Count out optimal location and speed.
On the basis of above-mentioned technical proposal, the present invention can also do following improvement.
Further, the step S3 is specifically included：
Three third order pll wave filters of design, three third order pll wave filters are respectively to X-axis, Y-axis and Z axis
Unmanned plane positional information is processed；
The coefficient of three branch roads of each third order pll wave filter is respectively C1、C2、C3, wherein, C1Branch road it is defeated
It is out1 to go outpll=errpos×C1, C2Branch road is output as out2pll=errpos×C2, C3Branch road is output as out3pll=∫
(errpos×C3) dt wherein errposRepresent the site error between reference position information and estimated location information.
Further, the step S3 also includes：To C2、C3The output of branch road sue for peace and obtains adding for phase-locked loop estimation
Velocity information, the acceleration information of as described estimation；
The acceleration information that the phase-locked loop is estimated is integrated, and and C1The output summation of branch road is estimated described in obtaining
The velocity information of the previous moment of meter.
Further, also comprised the following steps before the step S4：
T1, the measurement value information according to acceierometer sensor deducts zero specific force obtained under body coordinate system to the rear, passes through
Pose transformation matrix obtains the specific force under geographic coordinate system；Specific force under geographic coordinate system is deducted into gravity influence and obtains geographical seat
Movement acceleration information under mark system；To C2、C3The output of branch road sue for peace and obtains the acceleration information of phase-locked loop estimation；
T2, the acceleration that the movement acceleration information under the geographic coordinate system that will be obtained is estimated with the phase-locked loop
Degree information is overlapped, the acceleration information estimated after being aided in, the acceleration information of as described estimation；
The acceleration information of the estimation for obtaining is integrated, and and C1The output summation of branch road obtains the estimation
Previous moment velocity information.
Beneficial effect using above-mentioned further scheme is：The phase locked loop filter aided in using acceleration, is further subtracted
Small tracking error, especially in the scene of variable accelerated motion, therefore, it is possible to significantly improve tracking accuracy, that is, is obtained in that high-precision
The location estimation and velocity estimation of degree.
Further, the speed-displacement prediction model includes：
Current time speed prediction model, vel (n)=vel (n-1)+acc × T；
Current time position prediction model, pos (n)=pos (n-1)+vel (n-1) × T+2 1acc×T2；
Wherein, pos (n-1) is the positional information of the previous moment estimated, vel (n-1) is the previous moment of the estimation
Velocity information, acc is the acceleration information of the estimation, and T is the time cycle.
Further, three coefficients of branch road of the third order pll wave filter are according to the third order pll wave filter
Bandwidth determines.
Beneficial effect using above-mentioned further scheme is：Three parameter C1、C2、C3Can be entered according to loop tracks bandwidth
Row design, its method for designing is very classical, compared with the bearing calibration based on weight, parameter C1、C2、C3Physical significance more
Clearly, i.e., all it is the parameter related to bandwidth.
Further, the coefficient C of three branch roads of the third order pll wave filter1=2.4 ω0,WhereinBnIt is the bandwidth of the third order pll wave filter, ω0Frequency.
The present invention also provides a kind of position of Navigation of Pilotless Aircraft and the real-time estimation system of speed, and it includes：
Externally measured unit, the reference position information for obtaining unmanned plane；
Acceierometer sensor, for obtaining the movement acceleration information under unmanned body axis system；
Gyro, the attitude for obtaining unmanned plane realizes movement acceleration information under unmanned body axis system to geographical
The conversion of the movement acceleration information under coordinate system；
Third order pll wave filter, between the reference position information and estimated location information according to unmanned plane
The velocity information of the acceleration information of site error output estimation and the previous moment estimated；
Speed-displacement prediction unit, the acceleration of the estimation for being exported according to the third order pll wave filter
The velocity information of the estimation of information and the previous moment obtains the velocity information and positional information at current time.
Further, the externally measured unit includes satellite navigation receiver and barometertic altimeter.
Using the present invention provide Navigation of Pilotless Aircraft position and speed real-time estimation system can rapidly and accurately from
Optimal location and speed are estimated in external location information.
Brief description of the drawings
The flow chart of the real-time estimation method of the position and speed of the Navigation of Pilotless Aircraft that Fig. 1 is provided for the present invention；
Fig. 2 is the schematic diagram of the third order pll wave filter of present invention application；
Fig. 3 is square wave digital integrator of the present invention employed in discrete system；
Fig. 4 is speed, displacement prediction model of the present invention employed in discrete system；
Fig. 5 is that the acceleration of motion that be increased on the basis of third order pll wave filter basic structure that the present invention is used is aided in
Schematic diagram afterwards；
The schematic diagram of the real-time estimation method of the position and speed of the Navigation of Pilotless Aircraft that Fig. 6 present invention is provided.
Specific embodiment
Principle of the invention and feature are described below in conjunction with accompanying drawing, example is served only for explaining the present invention, and
It is non-for limiting the scope of the present invention.It should be noted that in the case where not conflicting, in embodiments herein and embodiment
Feature can be mutually combined.
Embodiment one
As shown in figure 1, a kind of position of Navigation of Pilotless Aircraft and the real-time estimation method of speed are present embodiments provided, its
Comprise the following steps：
S1, the reference position information of unmanned plane is obtained using externally measured unit；
S2, calculates the difference between current reference position information and speed-displacement prediction model estimated location information, obtains
Site error between the reference position information and the speed-displacement prediction model estimated location information；
S3, designs third order pll wave filter；
Position between the reference position information and the speed-displacement prediction model estimated location information that will obtain
Error as the third order pll wave filter input, through the treatment of the third order pll wave filter, output estimation plus
The velocity information of velocity information and the previous moment of estimation；
S4：Desin speed-displacement prediction model；
Using the velocity information of the acceleration information of the estimation and the previous moment of the estimation as the speed-displacement
The input of forecast model, through the speed that the treatment of the speed-displacement prediction model, output speed-displacement prediction model are estimated
The positional information that information and speed-displacement prediction model are estimated；
S5, returns to the step S1, circulates successively, realizes the tracking to the speed and position of unmanned plane.
Reference position information described above is the positional information of third order pll wave filter outside input, the reference position
Information is obtained by externally measured unit, such as by a direction of DVB acquisition or that barometertic altimeter is obtained
The positional information in elevation direction.
When the position of unmanned plane and speed tracing is carried out, first according to the initial bit in unmanned plane initiation time periods
Put, speed and acceleration obtain the positional information of initial time unmanned plane, meanwhile, externally measured unit obtains unmanned plane this moment
Reference position information, the reference position information of unmanned plane this moment is poor with the positional information of the unmanned plane of above-mentioned acquisition, obtain
Site error, using the site error as third order pll wave filter input, through the treatment of the third order pll wave filter,
Acceleration information and the velocity information of the previous moment of estimation that output phase-locked loop is estimated；And estimate the phase-locked loop
The velocity information of acceleration information and the previous moment of the estimation as the speed-displacement prediction model input, through institute
State the treatment of speed-displacement prediction model, the velocity information at the current time of output estimation and the positional information of estimation；Again should
The positional information of estimation is poor with the current reference position information that externally measured unit is obtained, and obtains reference position information and estimates
Site error between positional information, then above-mentioned steps S2, S3, S4 are carried out, circulate successively, realize the speed to unmanned plane and position
The tracking put.
The position of the Navigation of Pilotless Aircraft that the present embodiment is provided and the real-time estimation method of speed, it utilizes third order pll
Wave filter and speed-displacement prediction model estimate speed and the position of unmanned plane, compared with Kalman filter, its operand compared with
It is small, optimal location and speed can be rapidly and accurately estimated from external location information.
Embodiment two
The position of the Navigation of Pilotless Aircraft that the present embodiment is provided and the real-time estimation method of speed, it comprises the following steps：
S1, the reference position information of unmanned plane is obtained using externally measured unit；
S2, calculates the difference between reference position information and speed-displacement prediction model estimated location information, obtains described
Site error between reference position information and the speed-displacement prediction model estimated location information；
S3, designs third order pll wave filter；
Position between the reference position information and the speed-displacement prediction model estimated location information that will obtain
Error as the third order pll wave filter input, through the treatment of the third order pll wave filter, output estimation plus
The velocity information of velocity information and the previous moment of estimation；
S4：Desin speed-displacement prediction model；
The acceleration information and the velocity information of the previous moment of the estimation that the phase-locked loop is estimated are used as described
The input of speed-displacement prediction model, through the treatment of the speed-displacement prediction model, output current time speed-displacement is pre-
Survey the positional information that the velocity information and speed-displacement prediction model of model estimation are estimated；
S5, returns to the step S1, circulates successively, realizes the tracking to the speed and position of unmanned plane.
As shown in Figure 2, it is necessary to design three identical third order pll wave filters respectively to X-axis, Y-axis and Z axis position
Confidence breath is tracked, and each third order pll wave filter design procedure is：
Obtain the site error between the local position estimated and reference position, reference position (posref) refer to outside phaselocked loop
The positional information of portion's input, such as by elevation side in a direction of DVB acquisition or that barometertic altimeter is obtained
To positional information；The local position estimatedRefer to by being exported after third order pll wave filter and speed-displacement model
's.
Site error
Above-mentioned site error is respectively C respectively through three effects of branch road, the coefficient of three branch roads1、C2、C3, wherein,
C1The output out1 of branch roadpll=errpos×C1, C2Branch road is output as out2pll=errpos×C2, C3Branch road is output as
out3pll=∫ (errpos×C3) dt, wherein errposRepresent the position that reference position information is estimated with speed-displacement prediction model
Site error between information；
To C2、C3The output of branch road is sued for peace： Represent third order pll filtering
The acceleration information that average acceleration during last moment to this moment that device is estimated, as phase-locked loop are estimated, corresponding diagram
In A points.
In one of implementation method of the present embodiment, the acceleration information conduct of the phase-locked loop estimation of above-mentioned acquisition
The acceleration information of the estimation, the acceleration information to the estimation is integrated, i.e., With C after integration1Branch road
Output summation, that is, obtain the total output of third order pll wave filter
Three branch road coefficients of above-mentioned third order pll wave filter determine according to the bandwidth of third order pll wave filter, first
First, bandwidth B as needednDetermine frequencies omega0；
Further according to frequencies omega0Determine three coefficients of branch road, C1=2.4 ω0,
As shown in figure 3, one square wave digital integrator of design, therein is in integration inputs, and output is that integration is defeated
Go out.T is integration period, Z-1It is unit delay, according to the square wave digital integrator of design, is transported in third order pll wave filter
With two digital integrators, respectively digital integrator 1 and digital integrator 2, digital integrator 1 is realized to C3The product of branch road
Point, digital integrator 2 realizes the acceleration estimated looped phase locking loopIntegration.
One speed-displacement prediction model of redesign, as shown in figure 4, the corresponding formula of speed-displacement prediction model is such as
Under：
Vel (n)=vel (n-1)+acc × T
Wherein, the acceleration information that acc estimates for phase-locked loop, the velocity information of the previous moment that vel (n-1) estimates,
Vel (n) is the output of current time prediction of speed, and pos (n-1) is the positional information of the estimation of last moment, and pos (n) is current
The displacement output that moment is estimated, T is the time cycle.
Using above-mentioned processing method, the tracking of the speed and position of unmanned plane, and the entirely fortune of processing procedure are realized
Calculation amount is smaller, and optimal location and speed can be rapidly and accurately estimated from external location information.
Further to improve the speed and the tracking accuracy of position to unmanned plane, in another implementation method of the present embodiment,
Introduce acceleration of motion to aid in the estimated acceleration that above-mentioned third order pll wave filter is exported, i.e. in the step S4
Also comprise the following steps before：
T1, the measurement value information according to acceierometer sensor deducts zero specific force obtained under body coordinate system to the rear, passes through
Pose transformation matrix obtains the specific force under geographic coordinate system；Specific force under geographic coordinate system is deducted into gravity influence and obtains geographical seat
Movement acceleration information under mark system；And to C2、C3The output of branch road last moment to this when estimated that sue for peace, sets a date
Between average accelerator information, that is, obtain phase-locked loop estimation acceleration information；
T2, the acceleration that the acceleration information under the geographic coordinate system that will be obtained is estimated with the phase-locked loop is believed
Breath is overlapped, and obtains the acceleration information estimated after the auxiliary, and the acceleration information estimated after the auxiliary is as step
The acceleration information of the estimation in S4；
The acceleration information estimated after the auxiliary that will be obtained is integrated, and and C1The output summation of branch road obtains institute
State the velocity information of the previous moment of estimation.
Specifically, X-axis, Y-axis, the measured value of three accelerometers in direction of Z axis are designated as acc_measure under body coordinate system
(3-dimensional column vector), zero is designated as acc_bias (3-dimensional column vector) partially, and the specific force under body coordinate system is designated as(3-dimensional column vector), then
Have：Wherein,Acc_measure, acc_bias are 3-dimensional column vector, 3
Individual component corresponds to X-axis, Y-axis, Z axis respectively.
The measurement value information of the acceierometer sensor is deducted into zero specific force obtained under body coordinate system to the rear, by appearance
State transition matrix obtains the specific force under geographic coordinate system, and pose transformation matrix is obtained using gyro information using traditional maturation method
It is designated asSpecific force under geographic coordinate system is designated asThen have： It is 3-dimensional column vector, 3 components point
Dui Ying not X-axis, Y-axis, Z axis.
Gravitational vectors is expressed as in NED geographic coordinate systems：G=[0 0 g]T, the specific force under geographic coordinate system is deducted
The movement acceleration information under geographic coordinate system is obtained after gravity influence, acc is designated asn, then have：accnIt is
3-dimensional column vector, 3 components correspond to X-axis, Y-axis, Z axis respectively, it is assumed that accnThree representation in components be：accn=[accx accy
accz]T。
The wave filter on third order pll road is aided in using the acceleration information under the geographic coordinate system of above-mentioned acquisition,
Specially：
As shown in figure 5, by the acceleration of motion acc under geographic coordinate systemnRespective components be added to phase locked loop filter
Acceleration analysis point A, thenI.e.
Assuming that accnThree representation in components be：accn=[accx accy accz]T, then
Acceleration after X-axis auxiliary：I.e.
Acceleration after Y-axis auxiliary：I.e.
Acceleration after Z axis auxiliary：I.e.
The process of auxiliary is exactly the process of superposition, refers to that above three axle is superimposed accx, accy, accz respectively.
The acceleration information estimated after the auxiliary that will be obtained is integrated, and and C1The output summation of branch road obtains institute
The velocity information of the previous moment of estimation is stated, the acceleration information and the speed of the previous moment of the estimation of acquisition that will be estimated after auxiliary
Degree information as speed-displacement prediction model input, through the treatment of speed-displacement prediction model, output current time speed-
The positional information that the velocity information and speed-displacement prediction model that displacement prediction model is estimated are estimated.
Specifically, completed to the acceleration information after auxiliary by the digital integrator 2 in wave filterIntegration, integration
The speed of body estimation is obtained afterwardsAsAnd by the velocity information and C after integration1The output of branch road is asked
The output total with third order pll wave filter is obtainedObtain the previous moment of the estimation
Velocity information.As shown in fig. 6, position and the real-time estimation of speed it gives the preferred Navigation of Pilotless Aircraft of the present embodiment
The schematic diagram of method.As shown in fig. 6, wherein phase locked loop filter has used two digital integrators, it is respectively digital integrator 1
With digital integrator 2, digital integrator 1 realized to C3The integration of branch road.Digital integrator 2 realizes the acceleration estimated loopIntegration.
Calculate the speed of prediction：
The equation left sideIntegration is completed in the way of cumulative, the output of current time prediction of speed is obtainedIt is equivalent to
Vel (n) in above-mentioned formula, on the right of equationIt is the output of previous moment speed, has corresponded to the vel (n-1) of above-mentioned formula.
Calculate the position of prediction：
The equation left sideIntegration is completed in the way of cumulative, is the output of current time position prediction, be equivalent to above-mentioned public affairs
Pos (n) in formula, is equivalent to by after above formula integral and calculatingOn the right of equationIt is equivalent to the pos (n- of above-mentioned formula
1)。
Calculate current time site error：
posrefN () represents the external position reference input at current time,The position prediction at current time is represented, is come
After being integrated from above-mentioned formula
Bandwidth B as needednDetermine frequencies omega0：Bn=0.748 ω0,
Three coefficient C of branch road are determined according to frequency1、C2、C3。
C1=2.4 ω0,
Phaselocked loop C is calculated according to site error1Branch road exports out1pll；
out1pll=errpos(n)×C1；
Phaselocked loop C is calculated according to site error2Branch road exports out2pll；
out2pll=errpos(n)×C2；
Phaselocked loop C is calculated according to site error and digital integrator 13Branch road exports out3pll；
out3pll=out3pll+errpos(n)×C3×T；out3pllCompleted to err in the way of cumulativepos(n)×C3's
Integration.
The acceleration of the geographic coordinate system obtained by Inertial Measurement Unit, the acceleration of phaselocked loop is aided in using the acceleration
Degree branch road, obtains
Acceleration after X-axis auxiliary：
Acceleration after Y-axis auxiliary：
Acceleration after Z axis auxiliary：
Complete right by digital integrator 2Integration
out23pllComplete right in the way of cumulativeIntegration.
Calculate total output of third order pll wave filter：outpll=out23pll+out1pll；
Using total input exported with the acceleration after auxiliary as speed-displacement model of third order pll wave filter,
Obtain the velocity information and positional information estimated.
The position of the Navigation of Pilotless Aircraft that the present embodiment is provided and the real-time estimation method of speed, it uses acceleration to aid in
Phase locked loop filter, compared with Kalman filter, hence it is evident that reduce operand；And according to loop tracks bandwidth Design three
The parameter of branch road, compared with the bearing calibration based on weight, parameter C1、C2、C3Physical significance definitely, i.e., be all and band
Related parameter wide；By the design aided in using acceleration, tracking error is further reduced, especially in variable accelerated motion
Scene, tracking accuracy can be significantly improved, that is, be obtained in that high-precision location estimation and velocity estimation.
Embodiment three
The real-time estimation method of position and speed based on the Navigation of Pilotless Aircraft described in embodiment one or two, the present embodiment
The real-time estimation system of position and speed there is provided a kind of Navigation of Pilotless Aircraft, it includes：
Externally measured unit, the reference position information for obtaining unmanned plane；
Acceierometer sensor, for obtaining the movement acceleration information under unmanned body axis system；
Gyro, the attitude for obtaining unmanned plane realizes movement acceleration information under unmanned body axis system to geographical
The conversion of the movement acceleration information under coordinate system；
Third order pll wave filter, for the reference position information according to unmanned plane and the estimated location information it
Between site error output estimation acceleration information and estimate previous moment velocity information；
Speed-displacement prediction unit, the acceleration of the estimation for being exported according to the third order pll wave filter
The velocity information of the estimation of information and the previous moment obtains the velocity information and positional information at current time.
Externally measured unit therein includes satellite navigation receiver and barometertic altimeter.
In one of implementation method of the present embodiment, the acceleration information of estimation therein is by third order pll
C2、C3The output of branch road sue for peace the acceleration information that the phase-locked loop for obtaining is estimated, the acceleration that the phase-locked loop is estimated
Degree information is integrated, and and C1The output summation of branch road obtains the velocity information of the previous moment of the estimation.By what is obtained
The velocity information of the acceleration information of phase-locked loop estimation and the previous moment of the estimation for obtaining is used as speed-displacement prediction model
Input, by treatment obtain current time estimate velocity information and positional information.
In this another implementation method implemented, the ground obtained through treatment using the measurement value information of acceierometer sensor
Movement acceleration information under reason coordinate system is to the C by third order pll2、C3The output of branch road sue for peace the lock phase for obtaining
The acceleration information that loop is estimated is overlapped, the acceleration information estimated after being aided in, the acceleration information as estimated.
The acceleration information estimated after the auxiliary that will be obtained again is integrated, and and C1The output summation of branch road obtains the estimation
Previous moment velocity information.To obtain through the acceleration information after auxiliary and the speed of the previous moment of the estimation for obtaining
Information obtains the velocity information and positional information of current time estimation by treatment as the input of speed-displacement prediction model.
The position of Navigation of Pilotless Aircraft and the real-time estimation system of speed provided using the present embodiment can realize unmanned plane
Speed and the accurate tracking of position, reduce tracking error, obtain high-precision location estimation and velocity estimation
The foregoing is only presently preferred embodiments of the present invention, be not intended to limit the invention, it is all it is of the invention spirit and
Within principle, any modification, equivalent substitution and improvements made etc. should be included within the scope of the present invention.
Claims (9)
1. the real-time estimation method of a kind of position of Navigation of Pilotless Aircraft and speed, it is characterised in that including：
S1：The reference position information of unmanned plane is obtained using externally measured unit；
S2：The difference between reference position information and speed-displacement prediction model estimated location information is calculated, the reference is obtained
Site error between positional information and the speed-displacement prediction model estimated location information；
S3：Design third order pll wave filter；
Site error between the reference position information and the speed-displacement prediction model estimated location information that will obtain
As the input of the third order pll wave filter, through the treatment of the third order pll wave filter, the acceleration of output estimation
The velocity information of information and the previous moment of estimation；
S4：Desin speed-displacement prediction model；
Using the velocity information of the acceleration information of the estimation and the previous moment of the estimation as the speed-displacement prediction
The input of model, through the treatment of the speed-displacement prediction model, output current time speed-displacement prediction model is estimated
The positional information that velocity information and speed-displacement prediction model are estimated；
S5：The step S1 is returned, is circulated successively, realize the tracking to the speed and position of unmanned plane.
2. the real-time estimation method of the position of Navigation of Pilotless Aircraft according to claim 1 and speed, it is characterised in that institute
Step S3 is stated to specifically include：
Three third order pll wave filters of design, three third order pll wave filters respectively to X-axis, Y-axis and Z axis nobody
Machine positional information is processed；
The coefficient of three branch roads of each third order pll wave filter is respectively C1、C2、C3, wherein, C1Branch road is output as
out1pll=errpos×C1, C2Branch road is output as out2pll=errpos×C2, C3Branch road is output as out3pll=∫
(errpos×C3) dt, wherein errposBetween the positional information that expression reference position information and speed-displacement prediction model are estimated
Site error.
3. the real-time estimation method of the position of Navigation of Pilotless Aircraft according to claim 2 and speed, it is characterised in that institute
Stating step S3 also includes：
To C2、C3The output of branch road sue for peace and obtains the acceleration information of phase-locked loop estimation, the acceleration of as described estimation
Information；
The acceleration information that the phase-locked loop is estimated is integrated, and and C1The output summation of branch road obtains the estimation
The velocity information of previous moment.
4. the real-time estimation method of the position of Navigation of Pilotless Aircraft according to claim 2 and speed, it is characterised in that
Also comprise the following steps before the step S4：
T1, deducts zero specific force obtained under body coordinate system to the rear, by appearance by the measurement value information of the acceierometer sensor
State transition matrix obtains the specific force under geographic coordinate system；Specific force under geographic coordinate system is deducted into gravity influence and obtains geographical coordinate
Movement acceleration information under system；And to C2、C3The output of branch road sue for peace and obtains the acceleration information of phase-locked loop estimation；
T2, the acceleration that the movement acceleration information under the geographic coordinate system that will be obtained is estimated with the phase-locked loop is believed
Breath is overlapped, the acceleration information estimated after being aided in, the acceleration information of as described estimation；
The acceleration information estimated after the auxiliary that will be obtained is integrated, and and C1The output summation of branch road is estimated described in obtaining
The velocity information of the previous moment of meter.
5. the real-time estimation method of the position of the Navigation of Pilotless Aircraft according to any one of Claims 1-4 and speed, it is special
Levy and be, the speed-displacement prediction model includes：
Current time speed prediction model, vel (n)=vel (n-1)+acc × T；
Current time position prediction model,Wherein, pos
(n-1) be estimate previous moment positional information, vel (n-1) is the velocity information of the previous moment of the estimation, and acc is
The acceleration information of the estimation, T is the time cycle.
6. the real-time estimation method of the position of the Navigation of Pilotless Aircraft according to any one of claim 2 to 4 and speed, it is special
Levy and be, three coefficients of branch road of the third order pll wave filter are true according to the bandwidth of the third order pll wave filter
It is fixed.
7. the real-time estimation method of the position of Navigation of Pilotless Aircraft according to claim 6 and speed, it is characterised in that institute
State the coefficient C of three branch roads of third order pll wave filter1=2.4 ω0,WhereinBnIt is the bandwidth of the third order pll wave filter, ω0Frequency.
8. the real-time estimation system of a kind of position of Navigation of Pilotless Aircraft and speed, it is characterised in that including：
Externally measured unit, the reference position information for obtaining unmanned plane；
Acceierometer sensor, for obtaining the movement acceleration information under unmanned body axis system；
Gyro, the attitude for obtaining unmanned plane realizes movement acceleration information under unmanned body axis system to geographical coordinate
The conversion of the movement acceleration information under system；
Third order pll wave filter, for the position between the reference position information and estimated location information according to unmanned plane
The velocity information of the acceleration information of error output estimation and the previous moment estimated；
Speed-displacement prediction unit, the acceleration information of the estimation for being exported according to the third order pll wave filter
And the velocity information of the previous moment of the estimation obtains the velocity information and positional information at current time.
9. the real-time estimation system of the position of Navigation of Pilotless Aircraft according to claim 8 and speed, it is characterised in that institute
Stating externally measured unit includes satellite navigation receiver and barometertic altimeter.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201611051078.1A CN106767798B (en) | 2016-11-23 | 2016-11-23 | Real-time estimation method and system for position and speed for unmanned aerial vehicle navigation |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201611051078.1A CN106767798B (en) | 2016-11-23 | 2016-11-23 | Real-time estimation method and system for position and speed for unmanned aerial vehicle navigation |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN106767798A true CN106767798A (en) | 2017-05-31 |
| CN106767798B CN106767798B (en) | 2020-04-10 |
Family
ID=58912227
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201611051078.1A Active CN106767798B (en) | 2016-11-23 | 2016-11-23 | Real-time estimation method and system for position and speed for unmanned aerial vehicle navigation |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN106767798B (en) |
Cited By (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107576977A (en) * | 2017-09-08 | 2018-01-12 | 北京韦加无人机科技股份有限公司 | The UAV Navigation System and method adaptively merged based on multi-source information |
| CN108196565A (en) * | 2018-03-04 | 2018-06-22 | 西北工业大学 | A kind of novel unmanned plane and its attitude control method being combined based on projection with more rotors |
| CN111273671A (en) * | 2020-03-03 | 2020-06-12 | 大连海事大学 | Non-periodic communication remote observer of intelligent ship |
| CN111580553A (en) * | 2020-05-11 | 2020-08-25 | 桂林电子科技大学 | Unmanned aerial vehicle flight controller, unmanned aerial vehicle epidemic prevention supervision system and method |
| CN113779166A (en) * | 2021-08-20 | 2021-12-10 | 上海瑾盛通信科技有限公司 | Geo-fence control method and device, storage medium and electronic equipment |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103116038A (en) * | 2013-01-21 | 2013-05-22 | 中国人民解放军国防科学技术大学 | Acceleration-measuring method by satellite receiver carrier tracking l |
| CN103744100A (en) * | 2014-01-07 | 2014-04-23 | 北京航空航天大学 | Integrated navigation method based on satellite navigation and inertial navigation |
| CN105371838A (en) * | 2014-08-06 | 2016-03-02 | 航天恒星科技有限公司 | Combination navigation method and combination navigation system based on INS assisted GNSS single antenna attitude measurement |
| US20160195619A1 (en) * | 2015-01-02 | 2016-07-07 | Samsung Electronics Co., Ltd. | Adaptive gnss power saving control |
| CN105988129A (en) * | 2015-02-13 | 2016-10-05 | 南京理工大学 | Scalar-estimation-algorithm-based INS/GNSS combined navigation method |
-
2016
- 2016-11-23 CN CN201611051078.1A patent/CN106767798B/en active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103116038A (en) * | 2013-01-21 | 2013-05-22 | 中国人民解放军国防科学技术大学 | Acceleration-measuring method by satellite receiver carrier tracking l |
| CN103744100A (en) * | 2014-01-07 | 2014-04-23 | 北京航空航天大学 | Integrated navigation method based on satellite navigation and inertial navigation |
| CN105371838A (en) * | 2014-08-06 | 2016-03-02 | 航天恒星科技有限公司 | Combination navigation method and combination navigation system based on INS assisted GNSS single antenna attitude measurement |
| US20160195619A1 (en) * | 2015-01-02 | 2016-07-07 | Samsung Electronics Co., Ltd. | Adaptive gnss power saving control |
| CN105988129A (en) * | 2015-02-13 | 2016-10-05 | 南京理工大学 | Scalar-estimation-algorithm-based INS/GNSS combined navigation method |
Non-Patent Citations (1)
| Title |
|---|
| 石永锋等: "《大学物理（上）》", 31 January 2016 * |
Cited By (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107576977A (en) * | 2017-09-08 | 2018-01-12 | 北京韦加无人机科技股份有限公司 | The UAV Navigation System and method adaptively merged based on multi-source information |
| CN107576977B (en) * | 2017-09-08 | 2020-11-03 | 北京韦加无人机科技股份有限公司 | Unmanned aerial vehicle navigation system and method based on multi-source information self-adaptive fusion |
| CN108196565A (en) * | 2018-03-04 | 2018-06-22 | 西北工业大学 | A kind of novel unmanned plane and its attitude control method being combined based on projection with more rotors |
| CN108196565B (en) * | 2018-03-04 | 2020-11-06 | 西北工业大学 | Novel unmanned aerial vehicle based on combination of projection and multiple rotors and attitude control method thereof |
| CN111273671A (en) * | 2020-03-03 | 2020-06-12 | 大连海事大学 | Non-periodic communication remote observer of intelligent ship |
| CN111580553A (en) * | 2020-05-11 | 2020-08-25 | 桂林电子科技大学 | Unmanned aerial vehicle flight controller, unmanned aerial vehicle epidemic prevention supervision system and method |
| CN113779166A (en) * | 2021-08-20 | 2021-12-10 | 上海瑾盛通信科技有限公司 | Geo-fence control method and device, storage medium and electronic equipment |
| CN113779166B (en) * | 2021-08-20 | 2024-02-13 | 上海瑾盛通信科技有限公司 | Geofence control method and device, storage medium and electronic equipment |
Also Published As
| Publication number | Publication date |
|---|---|
| CN106767798B (en) | 2020-04-10 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106767798A (en) | A kind of position of Navigation of Pilotless Aircraft and the real-time estimation method of speed and system | |
| US11566901B2 (en) | Integrated navigation method for mobile vehicle | |
| CN110207697B (en) | Inertial navigation resolving method based on angular accelerometer/gyroscope/accelerometer | |
| CN104698485B (en) | Integrated navigation system and air navigation aid based on BD, GPS and MEMS | |
| JP5569681B2 (en) | POSITION ESTIMATION DEVICE AND POSITION ESTIMATION METHOD OF MOBILE BODY USING INDUCTION SENSOR, MAGNETIC SENSOR AND SPEED METER | |
| CN103917850B (en) | A kind of motion alignment methods of inertial navigation system | |
| Gautier | GPS/INS generalized evaluation tool (GIGET) for the design and testing of integrated navigation systems | |
| CN110887481B (en) | Carrier dynamic attitude estimation method based on MEMS inertial sensor | |
| CN109596018A (en) | Rotating missile flight attitude high-precision estimation method based on magnetic survey rolling angular rate information | |
| Rios et al. | Fusion filter algorithm enhancements for a MEMS GPS/IMU | |
| Oh | Multisensor fusion for autonomous UAV navigation based on the Unscented Kalman Filter with Sequential Measurement Updates | |
| US10871374B2 (en) | Estimating the speed and the heading of an aircraft, independently of a magnetic measurement | |
| CN105806343B (en) | Indoor 3D alignment systems and method based on inertial sensor | |
| CN111221347B (en) | Acceleration compensation method and system in attitude estimation of vertical take-off and landing fixed wing unmanned aerial vehicle | |
| CN107576977B (en) | Unmanned aerial vehicle navigation system and method based on multi-source information self-adaptive fusion | |
| CN110849360B (en) | Distributed relative navigation method for multi-machine collaborative formation flight | |
| ITTO20060107A1 (en) | AUTOMATIC PILOT EQUIPMENT FOR A ROTARY SAILING AIRCRAFT | |
| CN107289942A (en) | A kind of relative navigation system and method for formation flight | |
| CN113340298A (en) | Inertial navigation and dual-antenna GNSS external reference calibration method | |
| CN110095117A (en) | A kind of air navigation aid that gyro free inertia measurement system is combined with GPS | |
| RU2487318C1 (en) | Platform-free inertial attitude and heading reference system based on sensitive elements of medium accuracy | |
| CN113008229A (en) | Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor | |
| CN113447018B (en) | Real-time attitude estimation method of underwater inertial navigation system | |
| Wang et al. | An adaptive cascaded Kalman filter for two-antenna GPS/MEMS-IMU integration | |
| Narayanan | Performance analysis of attitude determination algorithms for low cost attitude heading reference systems |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |