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 PDF

Info

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
Application number
CN201611051078.1A
Other languages
Chinese (zh)
Other versions
CN106767798A (en
Inventor
罗兵
胡宝军
逯亮清
祝晓才
钱勇
何磊
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beijing Viga Uav Technology Co ltd
Original Assignee
Beijing Viga Uav Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beijing Viga Uav Technology Co ltd filed Critical Beijing Viga Uav Technology Co ltd
Priority to CN201611051078.1A priority Critical patent/CN106767798B/en
Publication of CN106767798A publication Critical patent/CN106767798A/en
Application granted granted Critical
Publication of CN106767798B publication Critical patent/CN106767798B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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

Real-time estimation method and system for position and speed for unmanned aerial vehicle navigation
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，
Figure BDA0001159972680000041
Wherein
Figure BDA0001159972680000042
BnIs 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 position
Figure BDA0001159972680000081
The output is output after passing through a third-order phase-locked loop filter and a speed-displacement model.
Position error
Figure BDA0001159972680000082
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:
Figure BDA0001159972680000083
Figure BDA0001159972680000084
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
Figure BDA0001159972680000085
Figure BDA0001159972680000086
Integrated and C1Summing the outputs of the branches to obtain the total output of the third-order phase-locked loop filter
Figure BDA0001159972680000087
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；
Figure BDA0001159972680000088
According to the frequency omega0Determining the coefficients of three branches, C1＝2.4ω0，
Figure BDA0001159972680000089
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 loop
Figure BDA0001159972680000092
Is 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
Figure BDA0001159972680000091
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)
Figure BDA0001159972680000101
(3-dimensional column vector) then there are:
Figure BDA0001159972680000102
wherein,
Figure BDA0001159972680000103
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 as
Figure BDA0001159972680000104
Specific force in the geographic coordinate system is recorded as
Figure BDA0001159972680000105
Then there are:
Figure BDA0001159972680000106
Figure BDA0001159972680000107
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:
Figure BDA0001159972680000108
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, then
Figure BDA0001159972680000109
Namely, it is
Figure BDA00011599726800001010
Let acc benIs represented as: acc (acrylic acid)n＝[accx accy accz]TThen, then
Acceleration after X-axis assistance:
Figure BDA00011599726800001011
namely, it is
Figure BDA00011599726800001012
Acceleration after Y-axis assistance:
Figure BDA00011599726800001013
namely, it is
Figure BDA0001159972680000111
Acceleration after Z-axis assistance:
Figure BDA0001159972680000112
namely, it is
Figure BDA0001159972680000113
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 filter
Figure BDA0001159972680000114
Is integrated to obtain the estimated speed of the body
Figure BDA0001159972680000115
Is that
Figure BDA0001159972680000116
And 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 filter
Figure BDA0001159972680000117
I.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 estimation
Figure BDA0001159972680000118
Is calculated.
Calculating the predicted speed:
Figure BDA0001159972680000119
left side of equation
Figure BDA00011599726800001110
The integral is completed in an accumulation mode to obtain the speed prediction output at the current moment
Figure BDA00011599726800001111
Equivalent to vel (n) in the above equation, right side of equation
Figure BDA00011599726800001112
Is the speed output at the previous moment, corresponding to vel (n-1) of the above formula.
Calculating the predicted position:
Figure BDA00011599726800001113
left side of equation
Figure BDA00011599726800001114
The 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 formula
Figure BDA00011599726800001115
Equation to the right
Figure BDA00011599726800001116
Equivalent to the abovePos (n-1) of the formula.
Calculating the position error at the current moment:
Figure BDA0001159972680000121
posref(n) represents an external position reference input at the current time,
Figure BDA0001159972680000122
position prediction representing the current time, from the integral of the above formula
Figure BDA0001159972680000123
Bandwidth B according to neednDetermining the frequency omega0：Bn＝0.748ω0，
Figure BDA0001159972680000124
Determining coefficients C of three branches according to frequency1、C2、C3。
C1＝2.4ω0，
Figure BDA0001159972680000125
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
Figure BDA0001159972680000126
Acceleration after X-axis assistance:
Figure BDA0001159972680000127
acceleration after Y-axis assistance:
Figure BDA0001159972680000128
acceleration after Z-axis assistance:
Figure BDA0001159972680000129
pair is completed by digital integrator 2
Figure BDA00011599726800001210
Integral of
Figure BDA00011599726800001211
out23pllComplete pairs in an accumulation manner
Figure BDA00011599726800001212
Is calculated.
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;
a model for predicting the position at the present time,
Figure FDA0002179114630000021
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.
3. The method of claim 2, wherein the coefficients C of the three branches of the third order PLL filter are C1＝2.4ω0，
Figure FDA0002179114630000022
Wherein
Figure FDA0002179114630000023
BnIs the bandwidth, omega, of the third order PLL filter0Is the frequency.
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;
a model for predicting the position at the present time,
Figure FDA0002179114630000041
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.
CN201611051078.1A 2016-11-23 2016-11-23 Real-time estimation method and system for position and speed for unmanned aerial vehicle navigation Active CN106767798B (en)

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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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

Patent Citations (4)

* Cited by examiner, † Cited by third party
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