CN106767798B - Real-time estimation method and system for position and speed for unmanned aerial vehicle navigation - Google Patents
Real-time estimation method and system for position and speed for unmanned aerial vehicle navigation Download PDFInfo
- Publication number
- CN106767798B CN106767798B CN201611051078.1A CN201611051078A CN106767798B CN 106767798 B CN106767798 B CN 106767798B CN 201611051078 A CN201611051078 A CN 201611051078A CN 106767798 B CN106767798 B CN 106767798B
- Authority
- CN
- China
- Prior art keywords
- estimated
- speed
- information
- position information
- locked loop
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 37
- 238000006073 displacement reaction Methods 0.000 claims abstract description 59
- 238000005259 measurement Methods 0.000 claims abstract description 17
- 230000001133 acceleration Effects 0.000 claims description 105
- 238000012545 processing Methods 0.000 claims description 18
- 238000004364 calculation method Methods 0.000 claims description 9
- 239000011159 matrix material Substances 0.000 claims description 7
- 230000005484 gravity Effects 0.000 claims description 6
- 230000009466 transformation Effects 0.000 claims description 6
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 239000013598 vector Substances 0.000 description 8
- 238000012937 correction Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 5
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 description 3
- 238000009825 accumulation Methods 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 3
- 230000010354 integration Effects 0.000 description 3
- 230000008569 process Effects 0.000 description 3
- SMZOUWXMTYCWNB-UHFFFAOYSA-N 2-(2-methoxy-5-methylphenyl)ethanamine Chemical compound COC1=CC=C(C)C=C1CCN SMZOUWXMTYCWNB-UHFFFAOYSA-N 0.000 description 2
- NIXOWILDQLNWCW-UHFFFAOYSA-N 2-Propenoic acid Natural products OC(=O)C=C NIXOWILDQLNWCW-UHFFFAOYSA-N 0.000 description 2
- 230000008901 benefit Effects 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 241000287196 Asthenes Species 0.000 description 1
- 239000000654 additive Substances 0.000 description 1
- 230000000996 additive effect Effects 0.000 description 1
- 238000000691 measurement method Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000003672 processing method Methods 0.000 description 1
- 230000007480 spreading Effects 0.000 description 1
Images
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 invention relates to a real-time estimation method and a real-time estimation system for position and speed of unmanned aerial vehicle navigation, wherein the method comprises the following steps of S1: acquiring current reference position information of the unmanned aerial vehicle by using an external measurement unit; s2: calculating a difference value between current reference position information and speed-displacement prediction model estimated position information to obtain a position error between the reference position information and the speed-displacement prediction model estimated position information; s3, designing a third-order phase-locked loop filter; s4: designing a speed-displacement prediction model; s5: and returning to the step S1, and circulating in sequence to realize the tracking of the speed and the position of the unmanned aerial vehicle. The method is simple in principle, easy to implement and popularize, and capable of quickly and accurately estimating the optimal position and speed from external position information.
Description
Technical Field
The invention relates to the technical field of unmanned aerial vehicle navigation, in particular to a method and a system for estimating position and speed for unmanned aerial vehicle navigation in real time.
Background
No matter be many rotor unmanned aerial vehicle or fixed wing unmanned aerial vehicle's control system all need provide high accuracy and real-time position, speed and attitude estimation, and high accuracy measurement is the prerequisite of high accuracy control, and the real-time nature then is the needs that have corresponded control cycle.
Although the satellite navigation receiver can provide position and speed information with errors not spreading over time, the update rate of data output is often far lower than the requirement of the flight control system, and the single-point positioning system cannot provide attitude information required by the flight control system. To provide real-time position, velocity and attitude information, the traditional method is a strapdown inertial navigation algorithm based on an inertial measurement unit (IMU, including a 3-axis gyroscope and a 3-axis accelerometer). Due to the requirement of low cost, the inertial measurement unit mostly adopts a micromechanical gyroscope and a micromechanical accelerometer. The navigation error can be diffused quickly due to the poor precision of the device. In order to suppress the increase of the error, the error of inertial navigation is usually corrected by using an absolute measurement method other than the IMU, such as a 3-axis magnetometer, a barometric altimeter and a satellite navigation receiver, and the method is characterized in that the error does not diverge with time.
A magnetic compass navigation algorithm based on the 3-axis magnetometer can provide a north reference and restrain the heading angle error of the inertial navigation system. The satellite-based navigation receiver can provide 3-dimensional position and speed information and restrain position, speed and horizontal attitude angle errors of an inertial navigation system. The altitude information based on the barometric altimeter can make up for the error of the satellite navigation receiver in the aspect of altitude positioning. The inertial navigation system after error correction can provide high-precision position, speed and attitude estimation and also meet the requirement of real-time property.
The method for suppressing the error of the inertial navigation system by using external absolute measurement information (such as a magnetic compass, a barometric altimeter, a satellite navigation receiver and the like) which does not diverge with time mainly adopts a Kalman filter. And estimating the state quantity in the state equation by establishing a state equation and an observation equation to obtain the filtered position, speed, attitude, gyro zero offset, accelerometer zero offset and the like. Typical methods are loose combinations based on position, velocity, tight combinations based on pseudoranges, pseudorange rates, etc.
However, the kalman filter has a large calculation amount, which is proportional to the square of the dimension of the filter, and is not suitable for a flight control system mainly including a single chip microcomputer. And applications in non-white noise conditions and unreasonable parameter settings tend to cause filter instability. Other correction methods than kalman filters have emerged, such as weight-based correction methods. The greatest advantage of this method is that the calculation amount is significantly lower than that of the kalman filter. The disadvantage is that the physical significance corresponding to the setting of the weight coefficient is not obvious.
Disclosure of Invention
In order to overcome the technical problems in the prior art, the invention provides a real-time position and speed estimation method for unmanned aerial vehicle navigation, which has the advantages of simple principle, easy realization and popularization, and can quickly and accurately estimate the optimal position and speed from external position information.
The technical scheme for solving the technical problems is as follows: a real-time estimation method of position and speed for unmanned aerial vehicle navigation comprises the following steps:
s1, acquiring the reference position information of the unmanned aerial vehicle by using an external measuring unit;
s2, calculating the difference between the reference position information and the estimated position information of the speed-displacement prediction model, and obtaining the position error between the reference position information and the estimated position information of the speed-displacement prediction model;
s3, designing a third-order phase-locked loop filter;
taking the position error between the obtained reference position information and the estimated position information of the speed-displacement prediction model as the input of the third-order phase-locked loop filter, and outputting the estimated acceleration information and the estimated speed information at the previous moment through the processing of the third-order phase-locked loop filter;
s4: designing a speed-displacement prediction model;
taking the estimated acceleration information and the estimated speed information at the previous moment as the input of the speed-displacement prediction model, and outputting the speed information estimated by the speed-displacement prediction model at the current moment and the position information estimated by the speed-displacement prediction model through the processing of the speed-displacement prediction model;
and S5, returning to the step S1, and circulating in sequence to realize the tracking of the speed and the position of the unmanned aerial vehicle.
The invention has the beneficial effects that: the speed and the position of the unmanned aerial vehicle are estimated by utilizing the third-order phase-locked loop filter and the speed-displacement prediction model, compared with a Kalman filter, the calculation amount is small, and the optimal position and speed can be quickly and accurately estimated from external position information.
On the basis of the technical scheme, the invention can be further improved as follows.
Further, the step S3 specifically includes:
designing three third-order phase-locked loop filters, wherein the three third-order phase-locked loop filters are used for respectively processing the position information of the unmanned aerial vehicle in an X axis, a Y axis and a Z axis;
the coefficients of three branches of each third-order phase-locked loop filter are respectively C1、C2、C3Wherein, C1The output of the branch is out1pll=errpos×C1,C2The output of the branch is out2pll=errpos×C2,C3The output of the branch is out3pll=∫(errpos×C3) dt where errposIndicating a position error between the reference position information and the estimated position information.
Further, the step S3 further includes: to C2、C3Summing the outputs of the branches to obtain the acceleration information estimated by the phase-locked loop, namely the estimated acceleration information;
integrating the estimated acceleration information of the phase-locked loop and C1And summing the outputs of the branches to obtain the estimated speed information at the previous moment.
Further, before the step S4, the method further includes the following steps:
t1, subtracting zero offset according to the measured value information of the accelerometer sensor to obtain the specific force under a body coordinate system, and obtaining the specific force under a geographic coordinate system through an attitude transformation matrix; deducting the gravity influence from the specific force under the geographic coordinate system to obtain the motion acceleration information under the geographic coordinate system; to C2、C3Summing the outputs of the branches to obtain the acceleration information estimated by the phase-locked loop;
t2, superimposing the obtained motion acceleration information in the geographic coordinate system and the acceleration information estimated by the phase-locked loop to obtain assisted estimated acceleration information, which is the estimated acceleration information;
integrating the estimated acceleration information obtained and C1And summing the outputs of the branches to obtain the estimated speed information at the previous moment.
The beneficial effect of adopting the further scheme is that: the acceleration-assisted phase-locked loop filter is adopted, so that the tracking error is further reduced, particularly in a variable acceleration motion scene, the tracking precision can be obviously improved, and high-precision position estimation and speed estimation can be obtained.
Further, the velocity-displacement prediction model includes:
a current-time speed prediction model, vel (n) ═ vel (n-1) + acc × T;
position prediction model at present time pos (n) ═ pos (n-1) + vel (n-1) × T +2 1acc×T2;
Pos (n-1) is position information of the previous estimated moment, vel (n-1) is speed information of the previous estimated moment, acc is the estimated acceleration information, and T is a time period.
Further, the coefficients of the three branches of the third-order phase-locked loop filter are determined according to the bandwidth of the third-order phase-locked loop filter.
The beneficial effect of adopting the further scheme is that: three parameters C1、C2、C3Can be designed according to the loop tracking bandwidth, the design method is very classical, and compared with a weight-based correction method, the parameter C1、C2、C3Are more definite, i.e. are all bandwidth related parameters.
Furthermore, the coefficients C of three branches of the third-order phase-locked loop filter1=2.4ω0,WhereinBnIs the bandwidth, omega, of the third order PLL filter0Frequency.
The invention also provides a real-time estimation system of the position and the speed for the navigation of the unmanned aerial vehicle, which comprises the following steps:
the external measurement unit is used for acquiring reference position information of the unmanned aerial vehicle;
the accelerometer sensor is used for acquiring motion acceleration information under an unmanned aerial vehicle body coordinate system;
the gyroscope is used for acquiring the attitude of the unmanned aerial vehicle and realizing the conversion from the motion acceleration information under the body coordinate system of the unmanned aerial vehicle to the motion acceleration information under the geographic coordinate system;
a third order phase-locked loop filter for outputting estimated acceleration information and estimated speed information of a previous moment according to a position error between the reference position information and the estimated position information of the unmanned aerial vehicle;
and the speed-displacement prediction unit is used for acquiring the speed information and the position information of the current moment according to the estimated acceleration information output by the third-order phase-locked loop filter and the estimated speed information of the previous moment.
Further, the external measurement unit includes a satellite navigation receiver and a barometric pressure gauge.
The real-time position and speed estimation system for unmanned aerial vehicle navigation can quickly and accurately estimate the optimal position and speed from external position information.
Drawings
Fig. 1 is a flowchart of a method for estimating a position and a velocity of an unmanned aerial vehicle for navigation in real time according to the present invention;
FIG. 2 is a schematic diagram of a third order PLL filter used in the present invention;
FIG. 3 is a rectangular wave digital integrator used in a discrete system in accordance with the present invention;
FIG. 4 is a velocity, displacement prediction model employed in the discrete system of the present invention;
FIG. 5 is a schematic diagram of the third-order PLL filter of the present invention with motion acceleration assistance;
fig. 6 is a schematic diagram of a real-time estimation method of position and speed for unmanned aerial vehicle navigation according to the present invention.
Detailed Description
The principles and features of this invention are described below in conjunction with the following drawings, which are set forth by way of illustration only and are not intended to limit the scope of the invention. It should be noted that the embodiments and features of the embodiments of the present application may be combined with each other without conflict.
Example one
As shown in fig. 1, the present embodiment provides a method for estimating a position and a speed for unmanned aerial vehicle navigation in real time, which includes the following steps:
s1, acquiring the reference position information of the unmanned aerial vehicle by using an external measuring unit;
s2, calculating the difference between the current reference position information and the estimated position information of the speed-displacement prediction model, and obtaining the position error between the reference position information and the estimated position information of the speed-displacement prediction model;
s3, designing a third-order phase-locked loop filter;
taking the position error between the obtained reference position information and the estimated position information of the speed-displacement prediction model as the input of the third-order phase-locked loop filter, and outputting the estimated acceleration information and the estimated speed information at the previous moment through the processing of the third-order phase-locked loop filter;
s4: designing a speed-displacement prediction model;
taking the estimated acceleration information and the estimated speed information at the previous moment as the input of the speed-displacement prediction model, and outputting the speed information estimated by the speed-displacement prediction model and the position information estimated by the speed-displacement prediction model through the processing of the speed-displacement prediction model;
and S5, returning to the step S1, and circulating in sequence to realize the tracking of the speed and the position of the unmanned aerial vehicle.
The reference position information is position information externally input by a third-order phase-locked loop filter, and the reference position information is acquired by an external measurement unit, such as position information in a certain direction acquired by a satellite receiver or position information in an elevation direction acquired by a barometric altimeter.
When the position and the speed of the unmanned aerial vehicle are tracked, firstly, the position information of the unmanned aerial vehicle at the initial time is obtained according to the initial position, the speed and the acceleration of the unmanned aerial vehicle in the initial time period, meanwhile, an external measuring unit obtains the reference position information of the unmanned aerial vehicle at the moment, the reference position information of the unmanned aerial vehicle at the moment is differed with the obtained position information of the unmanned aerial vehicle to obtain a position error, the position error is used as the input of a three-order phase-locked loop filter, and the acceleration information estimated by the phase-locked loop and the speed information estimated at the previous moment are output through the processing of the three-order phase-; taking the acceleration information estimated by the phase-locked loop and the speed information estimated at the previous moment as the input of the speed-displacement prediction model, and outputting the estimated speed information at the current moment and the estimated position information through the processing of the speed-displacement prediction model; and subtracting the estimated position information from the current reference position information obtained by the external measurement unit to obtain a position error between the reference position information and the estimated position information, and performing the steps S2, S3 and S4, and circulating in sequence to realize the tracking of the speed and the position of the unmanned aerial vehicle.
According to the real-time estimation method for the position and the speed of the unmanned aerial vehicle for navigation, the third-order phase-locked loop filter and the speed-displacement prediction model are used for estimating the speed and the position of the unmanned aerial vehicle, compared with a Kalman filter, the calculation amount is small, and the optimal position and speed can be quickly and accurately estimated from external position information.
Example two
The method for estimating the position and the speed of the unmanned aerial vehicle for navigation provided by the embodiment comprises the following steps:
s1, acquiring the reference position information of the unmanned aerial vehicle by using an external measuring unit;
s2, calculating the difference between the reference position information and the estimated position information of the speed-displacement prediction model, and obtaining the position error between the reference position information and the estimated position information of the speed-displacement prediction model;
s3, designing a third-order phase-locked loop filter;
taking the position error between the obtained reference position information and the estimated position information of the speed-displacement prediction model as the input of the third-order phase-locked loop filter, and outputting the estimated acceleration information and the estimated speed information at the previous moment through the processing of the third-order phase-locked loop filter;
s4: designing a speed-displacement prediction model;
taking the acceleration information estimated by the phase-locked loop and the speed information estimated at the previous moment as the input of the speed-displacement prediction model, and outputting the speed information estimated by the speed-displacement prediction model at the current moment and the position information estimated by the speed-displacement prediction model after the processing of the speed-displacement prediction model;
and S5, returning to the step S1, and circulating in sequence to realize the tracking of the speed and the position of the unmanned aerial vehicle.
As shown in fig. 2, three identical third-order pll filters are required to be designed to track the position information of the X axis, the Y axis, and the Z axis, respectively, and each third-order pll filter is designed by the following steps:
obtaining a position error between a locally estimated position and a reference position, the reference position (pos)ref) Position information input from the outside of the phase-locked loop, such as position information in a certain direction obtained by a satellite receiver or position information in an elevation direction obtained by a barometric altimeter; locally estimated positionThe output is output after passing through a third-order phase-locked loop filter and a speed-displacement model.
The position error is respectively acted by three branches, and the coefficients of the three branches are respectively C1、C2、C3Wherein, C1Output out1 of the branchpll=errpos×C1,C2The output of the branch is out2pll=errpos×C2,C3The output of the branch is out3pll=∫(errpos×C3) dt, err thereinposRepresenting reference position information and velocity-displacementPredicting a position error between the model estimated position information;
to C2、C3The outputs of the branches are summed: the average acceleration of the third-order phase-locked loop filter during the period from the last moment to the present moment is represented, that is, the acceleration information estimated by the phase-locked loop corresponds to the point a in the diagram.
In one embodiment of this embodiment, the obtained estimated acceleration information of the phase-locked loop is used as the estimated acceleration information, and the estimated acceleration information is integrated, that is, the integrated estimated acceleration information is obtained by integrating the obtained estimated acceleration information Integrated and C1Summing the outputs of the branches to obtain the total output of the third-order phase-locked loop filter
The three branch coefficients of the third-order phase-locked loop filter are determined according to the bandwidth of the third-order phase-locked loop filter, and firstly, the three branch coefficients are determined according to the required bandwidth BnDetermining the frequency omega0;
As shown in FIG. 3, a rectangular wave digital integrator is designed, in which is the in integral input and output isAnd (4) outputting the integral. T is the integration period, Z-1The method is characterized in that unit delay is adopted, two digital integrators, namely a digital integrator 1 and a digital integrator 2 are applied to a third-order phase-locked loop filter according to a designed rectangular wave digital integrator, and the digital integrator 1 realizes the C-pair3Integration of the arms, digital integrator 2 effecting the acceleration estimated for the loop phase-locked loopIs calculated.
And designing a speed-displacement prediction model, wherein as shown in fig. 4, the formula corresponding to the speed-displacement prediction model is as follows:
vel(n)=vel(n-1)+acc×T
the acc is acceleration information estimated by a phase-locked loop, the vel (n-1) is estimated speed information of the previous moment, the vel (n) is speed prediction output of the current moment, pos (n-1) is estimated position information of the previous moment, pos (n) is estimated displacement output of the current moment, and T is a time period.
By using the processing method, the tracking of the speed and the position of the unmanned aerial vehicle is realized, the calculation amount in the whole processing process is small, and the optimal position and speed can be quickly and accurately estimated from the external position information.
In order to further improve the tracking accuracy of the speed and the position of the drone, in another implementation of this embodiment, the estimated acceleration output by the third-order phase-locked loop filter is assisted by introducing the motion acceleration, that is, before the step S4, the following steps are further included:
t1, subtracting zero offset according to the measured value information of the accelerometer sensor to obtain the specific force under a body coordinate system, and obtaining the specific force under a geographic coordinate system through an attitude transformation matrix; deducting the gravity influence from the specific force under the geographic coordinate system to obtain the motion acceleration information under the geographic coordinate system; and to C2、C3Summing the outputs of the branches to obtain the estimated last moment to the current momentObtaining the average acceleration information during the time period, namely obtaining the acceleration information estimated by the phase-locked loop;
t2, superimposing the obtained acceleration information in the geographic coordinate system and the acceleration information estimated by the phase-locked loop to obtain the post-assistance estimated acceleration information, which is the estimated acceleration information in step S4;
integrating the obtained post-assisted estimated acceleration information with C1And summing the outputs of the branches to obtain the estimated speed information at the previous moment.
Specifically, the measurement values of the accelerometer in the three directions of the X axis, the Y axis and the Z axis in the body coordinate system are denoted as acc _ measure (3-dimensional column vector), the zero offset is denoted as acc _ bias (3-dimensional column vector), and the specific force in the body coordinate system is denoted as acc _ bias (3-dimensional column vector)(3-dimensional column vector) then there are:wherein,the acc _ measure and the acc _ bias are 3-dimensional column vectors, and 3 components respectively correspond to an X axis, a Y axis and a Z axis.
Deducting the measurement value information of the accelerometer sensor from zero offset to obtain the specific force under a body coordinate system, obtaining the specific force under a geographic coordinate system through an attitude transformation matrix, and obtaining the attitude transformation matrix by using gyroscope information through a traditional mature method and recording the attitude transformation matrix asSpecific force in the geographic coordinate system is recorded asThen there are: is a 3-dimensional column vector, and 3 components correspond to the X-axis, Y-axis, and Z-axis, respectively.
The gravity vector is expressed in the NED geographic coordinate system as: g ═ 00G]TSubtracting the gravity influence from the specific force in the geographic coordinate system to obtain the motion acceleration information in the geographic coordinate system, and recording as accnThen, there are:accnis a 3-dimensional column vector, 3 components correspond to the X-axis, Y-axis, and Z-axis, respectively, assuming accnIs represented as: acc (acrylic acid)n=[accxaccy accz]T。
The obtained acceleration information under the geographic coordinate system is used for assisting a filter of a third-order phase-locked loop, and the method specifically comprises the following steps:
as shown in fig. 5, the motion acceleration acc in the geographic coordinate system is determinednIs superposed to the acceleration measurement point a of the phase-locked loop filter, thenNamely, it is
Let acc benIs represented as: acc (acrylic acid)n=[accx accy accz]TThen, then
The auxiliary process is a stacking process, which means that the three axes are stacked accx, accy and accz respectively.
Integrating the obtained post-assisted estimated acceleration information with C1And summing the outputs of the branches to obtain the estimated speed information of the previous moment, taking the accelerated speed information estimated after the assistance and the obtained estimated speed information of the previous moment as the input of a speed-displacement prediction model, and outputting the speed information estimated by the speed-displacement prediction model at the current moment and the position information estimated by the speed-displacement prediction model through the processing of the speed-displacement prediction model.
In particular, the assisted acceleration information is done by a digital integrator 2 in the filterIs integrated to obtain the estimated speed of the bodyIs thatAnd the integrated speed information is compared with C1Summing the outputs of the branches to obtain the total output of the third-order phase-locked loop filterI.e. the estimated speed information of the previous moment. As shown in fig. 6, this figure shows the preferred drone navigation of the present embodimentSchematic diagram of a real-time estimation method of position and velocity. As shown in fig. 6, the pll filter employs two digital integrators, i.e. digital integrator 1 and digital integrator 2, and digital integrator 1 implements pair C3The integral of the branch. Digital integrator 2 implements acceleration on loop estimationIs calculated.
left side of equationThe integral is completed in an accumulation mode to obtain the speed prediction output at the current momentEquivalent to vel (n) in the above equation, right side of equationIs the speed output at the previous moment, corresponding to vel (n-1) of the above formula.
left side of equationThe integration is completed in an accumulation mode, the current time position prediction output is equivalent to pos (n) in the formula, and the integration is equivalent to the po (n) in the formula after the integral calculation of the formulaEquation to the rightEquivalent to the abovePos (n-1) of the formula.
posref(n) represents an external position reference input at the current time,position prediction representing the current time, from the integral of the above formula
Determining coefficients C of three branches according to frequency1、C2、C3。
Calculating a phase-locked loop C from the position error1Branch output out1pll;
out1pll=errpos(n)×C1;
Calculating a phase-locked loop C from the position error2Branch output out2pll;
out2pll=errpos(n)×C2;
Calculating a phase locked loop C from the position error and the digital integrator 13Branch output out3pll;
out3pll=out3pll+errpos(n)×C3×T;out3pllCompleting err pairs in an additive mannerpos(n)×C3Is calculated.
The acceleration of the geographic coordinate system obtained by the inertial measurement unit is utilized to assist an acceleration branch of the phase-locked loop to obtain the acceleration
Calculating the total output of the third order phase-locked loop filter: outpll=out23pll+out1pll;
And taking the total output of the third-order phase-locked loop filter and the assisted acceleration as the input of a speed-displacement model to obtain estimated speed information and position information.
According to the real-time estimation method for the position and the speed of the unmanned aerial vehicle for navigation, the phase-locked loop filter assisted by acceleration is adopted, and compared with a Kalman filter, the calculation amount is obviously reduced; and designing parameters of three branches according to the loop tracking bandwidthParameter C, in contrast to the weight-based correction method1、C2、C3The physical meanings of (A) are more definite, namely all parameters related to the bandwidth; by adopting the acceleration-assisted design, the tracking error is further reduced, and particularly in the scene of variable acceleration motion, the tracking precision can be obviously improved, namely the high-precision position estimation and speed estimation can be obtained.
EXAMPLE III
Based on the real-time estimation method of position and speed for unmanned aerial vehicle navigation described in the first or second embodiment, the present embodiment provides a real-time estimation system of position and speed for unmanned aerial vehicle navigation, which includes:
the external measurement unit is used for acquiring reference position information of the unmanned aerial vehicle;
the accelerometer sensor is used for acquiring motion acceleration information under an unmanned aerial vehicle body coordinate system;
the gyroscope is used for acquiring the attitude of the unmanned aerial vehicle and realizing the conversion from the motion acceleration information under the body coordinate system of the unmanned aerial vehicle to the motion acceleration information under the geographic coordinate system;
a third order phase-locked loop filter for outputting estimated acceleration information and estimated speed information of a previous moment according to a position error between the reference position information and the estimated position information of the unmanned aerial vehicle;
and the speed-displacement prediction unit is used for acquiring the speed information and the position information of the current moment according to the estimated acceleration information output by the third-order phase-locked loop filter and the estimated speed information of the previous moment.
The external measuring unit comprises a satellite navigation receiver and a barometric pressure gauge.
In one embodiment of this embodiment, the estimated acceleration information is C passed through a third-order phase-locked loop2、C3The acceleration information estimated by the phase-locked loop is obtained by summing the outputs of the branches, is integrated with the acceleration information estimated by the phase-locked loop and is C1And summing the outputs of the branches to obtain the estimated speed information at the previous moment. Phase locked loop to be obtainedAnd the acceleration information of the road estimation and the obtained speed information of the estimated previous moment are used as the input of a speed-displacement prediction model, and the speed information and the position information of the current moment are obtained through processing.
In another embodiment of the present embodiment, the motion acceleration information in the geographic coordinate system obtained by processing the measured value information of the accelerometer sensor is used for the C passing through the third-order phase-locked loop2、C3And the outputs of the branches are summed to obtain the acceleration information estimated by the phase-locked loop, and the acceleration information estimated after the assistance is obtained, namely the estimated acceleration information. Integrating the obtained acceleration information estimated after the assistance and C1And summing the outputs of the branches to obtain the estimated speed information at the previous moment. And processing the obtained assisted acceleration information and the obtained estimated speed information at the previous moment as the input of a speed-displacement prediction model to obtain the estimated speed information and the position information at the current moment.
The real-time position and speed estimation system for unmanned aerial vehicle navigation provided by the embodiment can realize accurate tracking of the speed and the position of the unmanned aerial vehicle, reduce tracking errors and obtain high-precision position estimation and speed estimation
The above description is only for the purpose of illustrating the preferred embodiments of the present invention and is not to be construed as limiting the invention, and any modifications, equivalents, improvements and the like that fall within the spirit and principle of the present invention are intended to be included therein.
Claims (5)
1. A real-time estimation method for position and speed for unmanned aerial vehicle navigation is characterized by comprising the following steps:
s1: acquiring reference position information of the unmanned aerial vehicle by using an external measurement unit;
s2: calculating a difference value between reference position information and estimated position information of a speed-displacement prediction model to obtain a position error between the reference position information and the estimated position information of the speed-displacement prediction model;
s3: designing a third-order phase-locked loop filter;
taking the position error between the obtained reference position information and the estimated position information of the speed-displacement prediction model as the input of the third-order phase-locked loop filter, and outputting the estimated acceleration information and the estimated speed information at the previous moment through the processing of the third-order phase-locked loop filter;
s4: designing a speed-displacement prediction model;
taking the estimated acceleration information and the estimated speed information at the previous moment as the input of the speed-displacement prediction model, and outputting the speed information and the position information estimated by the speed-displacement prediction model at the current moment through the processing of the speed-displacement prediction model;
s5: returning to the step S1, and circulating in sequence to realize the tracking of the speed and the position of the unmanned aerial vehicle;
the step S3 specifically includes:
designing three third-order phase-locked loop filters, wherein the three third-order phase-locked loop filters are used for respectively processing the position information of the unmanned aerial vehicle in an X axis, a Y axis and a Z axis;
the coefficients of three branches of each third-order phase-locked loop filter are respectively C1、C2、C3Wherein, C1The output of the branch is out1pll=errpos×C1,C2The output of the branch is out2pll=errpos×C2,C3The output of the branch is out3pll=∫(errpos×C3) dt, the specific calculation is: out3pll(n)=out3pll(n-1)+errpos(n)×C3X T; wherein errposRepresenting a position error between the reference position information and position information estimated by the velocity-displacement prediction model; out3pll(n) is the current time C3Output of branch, out3pll(n-1) is the previous time C3The output of the branch circuit;
the method further comprises the following steps before the step S4:
t1, subtracting the zero offset from the measured value of the accelerometer sensor to obtain the specific force in the body coordinate system, and converting the matrix through the attitudeObtaining specific force under a geographic coordinate system; deducting the gravity influence from the specific force under the geographic coordinate system to obtain the motion acceleration information under the geographic coordinate system; and to C2、C3Summing the outputs of the branches to obtain the acceleration information estimated by the phase-locked loop;
t2, superimposing the obtained motion acceleration information in the geographic coordinate system and the acceleration information estimated by the phase-locked loop, and obtaining the post-assistance estimated acceleration information as the estimated acceleration information;
integrating the obtained post-assisted estimated acceleration information with C1Summing the outputs of the branches to obtain the estimated speed information at the previous moment;
the velocity-displacement prediction model includes:
a current-time speed prediction model, vel (n) ═ vel (n-1) + acc × T;
pos (n-1) is position information of the previous estimated moment, vel (n-1) is speed information of the previous estimated moment, acc is the estimated acceleration information, and T is a time period.
2. The method of claim 1, wherein the coefficients of the three branches of the third order PLL filter are determined according to a bandwidth of the third order PLL filter.
4. The utility model provides a position and speed's real-time estimation system that unmanned aerial vehicle navigation was used which characterized in that includes:
the external measurement unit is used for acquiring reference position information of the unmanned aerial vehicle;
the accelerometer sensor is used for acquiring motion acceleration information under an unmanned aerial vehicle body coordinate system;
the gyroscope is used for acquiring the attitude of the unmanned aerial vehicle and realizing the conversion from the motion acceleration information under the body coordinate system of the unmanned aerial vehicle to the motion acceleration information under the geographic coordinate system;
a third order phase-locked loop filter for outputting estimated acceleration information and estimated speed information of a previous moment according to a position error between the reference position information and the estimated position information of the unmanned aerial vehicle;
the three third-order phase-locked loop filters are used for processing the position information of the unmanned aerial vehicle on the X axis, the Y axis and the Z axis respectively;
the coefficients of three branches of each third-order phase-locked loop filter are respectively C1、C2、C3Wherein, C1The output of the branch is out1pll=errpos×C1,C2The output of the branch is out2pll=errpos×C2,C3The output of the branch is out3pll=∫(errpos×C3) dt, the specific calculation is: out3pll(n)=out3pll(n-1)+errpos(n)×C3X T; wherein errposRepresenting a position error between the reference position information and position information estimated by the velocity-displacement prediction model; out3pll(n) is the current time C3Output of branch, out3pll(n-1) is the previous time C3The output of the branch circuit;
deducting the zero offset from the measured value of the accelerometer sensor to obtain the body coordinate systemThe specific force is obtained under a geographic coordinate system through the attitude transformation matrix; deducting the gravity influence from the specific force under the geographic coordinate system to obtain the motion acceleration information under the geographic coordinate system; and to C2、C3Summing the outputs of the branches to obtain the acceleration information estimated by the phase-locked loop;
superposing the obtained motion acceleration information under the geographic coordinate system and the acceleration information estimated by the phase-locked loop to obtain the assisted estimated acceleration information which is used as the estimated acceleration information;
integrating the obtained post-assisted estimated acceleration information with C1Summing the outputs of the branches to obtain the estimated speed information at the previous moment;
the speed-displacement prediction unit is used for obtaining speed information and position information of the current moment according to the estimated acceleration information output by the third-order phase-locked loop filter and the estimated speed information of the previous moment;
also for designing a velocity-displacement prediction model;
the velocity-displacement prediction model includes:
a current-time speed prediction model, vel (n) ═ vel (n-1) + acc × T;
pos (n-1) is position information of the previous estimated moment, vel (n-1) is speed information of the previous estimated moment, acc is the estimated acceleration information, and T is a time period.
5. The system of claim 4, wherein the external measurement unit comprises a satellite navigation receiver and a barometric pressure gauge.
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 CN106767798A (en) | 2017-05-31 |
CN106767798B true 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) |
Families Citing this family (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN107576977B (en) * | 2017-09-08 | 2020-11-03 | 北京韦加无人机科技股份有限公司 | Unmanned aerial vehicle navigation system and method based on multi-source information self-adaptive fusion |
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 |
CN111830518A (en) * | 2019-04-22 | 2020-10-27 | 苏州翼搏特智能科技有限公司 | Relative pose measurement system and method based on near-infrared beacon |
CN111273671B (en) * | 2020-03-03 | 2022-05-31 | 大连海事大学 | 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 |
CN113779166B (en) * | 2021-08-20 | 2024-02-13 | 上海瑾盛通信科技有限公司 | Geofence control method and device, storage medium and electronic equipment |
Citations (4)
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 |
CN105988129A (en) * | 2015-02-13 | 2016-10-05 | 南京理工大学 | Scalar-estimation-algorithm-based INS/GNSS combined navigation method |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US9804271B2 (en) * | 2015-01-02 | 2017-10-31 | Samsung Electronics Co., Ltd | Adaptive GNSS power saving control |
-
2016
- 2016-11-23 CN CN201611051078.1A patent/CN106767798B/en active Active
Patent Citations (4)
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 |
CN105988129A (en) * | 2015-02-13 | 2016-10-05 | 南京理工大学 | Scalar-estimation-algorithm-based INS/GNSS combined navigation method |
Also Published As
Publication number | Publication date |
---|---|
CN106767798A (en) | 2017-05-31 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106767798B (en) | Real-time estimation method and system for position and speed for unmanned aerial vehicle navigation | |
CN106052685B (en) | A kind of posture and course estimation method of two-stage separation fusion | |
CN110887480B (en) | Flight attitude estimation method and system based on MEMS sensor | |
CN100405014C (en) | Carrier attitude measurement method | |
CN106405670B (en) | A kind of gravity anomaly data processing method suitable for strapdown marine gravitometer | |
CN106482734A (en) | A kind of filtering method for IMU Fusion | |
CN104764467B (en) | Re-entry space vehicle inertial sensor errors online adaptive scaling method | |
CN104698485B (en) | Integrated navigation system and air navigation aid based on BD, GPS and MEMS | |
CN111551174A (en) | High-dynamic vehicle attitude calculation method and system based on multi-sensor inertial navigation system | |
CN104898681A (en) | Tetra-rotor aircraft attitude obtaining method by use of three-order approximation Picard quaternion | |
CN103941274B (en) | Navigation method and terminal | |
CN112504275B (en) | Water surface ship horizontal attitude measurement method based on cascade Kalman filtering algorithm | |
CN112066985B (en) | Initialization method, device, medium and electronic equipment for combined navigation system | |
CN102937450B (en) | A kind of relative attitude defining method based on gyro to measure information | |
CN106370178B (en) | Attitude measurement method and device of mobile terminal equipment | |
CN107576977B (en) | Unmanned aerial vehicle navigation system and method based on multi-source information self-adaptive fusion | |
CN112146655A (en) | Elastic model design method for BeiDou/SINS tight integrated navigation system | |
CN110941285A (en) | Unmanned aerial vehicle flight control system based on two IP cores | |
CN103712598A (en) | Attitude determination system and method of small unmanned aerial vehicle | |
CN108871319B (en) | Attitude calculation method based on earth gravity field and earth magnetic field sequential correction | |
CN111220151B (en) | Inertia and milemeter combined navigation method considering temperature model under load system | |
CN111912427B (en) | Method and system for aligning motion base of strapdown inertial navigation assisted by Doppler radar | |
CN108416387B (en) | Height filtering method based on fusion data of GPS and barometer | |
CN103901459B (en) | The filtering method of Measurement delay in a kind of MEMS/GPS integrated navigation system | |
CN113008229A (en) | Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor |
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 |