TWI422825B - Method and apparatus for high-precision velocity estimation - Google Patents

Description

High-precision speed estimation method and device thereof

The present invention relates to a speed estimation method and apparatus thereof, and more particularly to a speed estimation method and apparatus for accurately estimating a flight vehicle.

Since the position and speed of the flying vehicle directly affect the control of the flying vehicle, and the speed of the flying vehicle is usually not directly detected by the general speed sensor, it must be obtained by other available variables. For example, US Patent No. 4,167,330 discloses the use of radio waves in two directions to receive a delay in time to estimate the speed of the helicopter; however, due to weather or terrain, especially during small movements. The accuracy of the estimation is not high; and the influence of the acceleration of the helicopter on the three-axis (X, Y, Z) of the body is not added to the calculation, which makes it practically limited. Furthermore, Taiwan Patent TW I294376 discloses the use of two accelerometers to measure the longitudinal acceleration and lateral acceleration of the vehicle, and then use the integral to determine the speed of the vehicle's motion; although this method can eliminate the direct measurement speed. However, the acceleration integral is limited by the speed accuracy of the integral starting point, which is easy to cause large errors due to the accumulation of the DC offset. If it is applied to a flying vehicle, it is difficult to achieve an accurate purpose. According to the Taiwan patent TWI531657, it is disclosed that the global positioning system (GPS) raw observation and the inertial sensor receiving inertial measurement value are received by using a global positioning system processor, The speed and acceleration information is obtained. Although this method has feedback compensation, the accuracy of the speed estimation can be improved. However, for the noise cancellation and the failure to correct the three-dimensional acceleration component of the flight vehicle body, the accuracy is still insufficient.

As mentioned above, in the prior art, the flying vehicle can use satellite navigation to obtain the displacement values at different time points through the signals of the satellites, or use the global positioning system to obtain the displacement values of the flying vehicles, and then use the differential value of the displacement values. The operation obtains the velocity value; another conventional technique uses an inertial measurement unit (IMU) to obtain an acceleration value, and then integrates the operation to obtain a velocity value. However, regardless of the differential operation or the integral operation, there is an operational error due to high frequency noise or low frequency DC drift (initial value), making accurate speed values difficult to obtain. For example, U.S. Patent No. 6,205,400 discloses the use of a global positioning system, an inertial gauge, and an altimeter, and uses a Kalman filter to filter out noise to obtain information on the position, attitude, velocity, and acceleration of the flying vehicle. However, since the displacement value of the flight vehicle measured by the satellite navigation system or the global positioning system has a certain error, when the differential value is calculated, the error value will be enlarged and the noise will be amplified, so that the speed estimation is still Hard to be precise. If the flight vehicle uses an accelerometer to measure the acceleration value, although it is possible to avoid the difficulty of the terrain and terrain affecting the reception of the satellite signal, the speed value and the displacement value obtained by the integral acceleration calculation method may be due to the accumulation of the offset error amount. The longer the time is, the larger the offset error is. In 1994, Wang Xiuping wrote "Adaptive Kalman Filter for Flight Target Tracking System" and Lin Yongcheng's 2007 book "Using Neural Networks to Develop Hybrid INS/GPS Integrated Positioning and Orientation Algorithms" , respectively, proposed that the Kalman filter needs to adjust the variance and complex factors over a long period of time. (covariance), can be more accurate, which is quite time-consuming and costly work, especially when the flight vehicle (controlled system) changes, all fine tuning work needs to be reset. Therefore, how to develop an effective and accurate speed estimator is a very important technology; especially in the application of computerized unmanned aerial vehicles (UAVs) or autonomous unmanned aerial vehicles, accurate Speed estimates are extremely important for control performance. Without good speed estimation capabilities, not only can high performance mobile or flight control be achieved, but also hidden flight crises; therefore, how to accurately estimate unmanned aerial vehicles Displacement values, velocity values, and acceleration values are issues that need to be addressed.

In view of the above-mentioned problems of the prior art, the object of the present invention is to provide a high-precision speed estimation method to solve the problem caused by excessive error when the flying vehicle measures the speed value.

According to one of the main purposes of the present invention, a high-precision speed estimation method is proposed, which can estimate the value of the estimated speed of the current time from the value of the displacement measurement signal obtained by the previous time measurement and the value of the acceleration measurement signal; The displacement measurement signal is sent by the displacement sensor of the flight vehicle, and the acceleration measurement signal is sent by the acceleration sensor of the flight vehicle; the following steps are included: S1: The acceleration measurement signal of the previous time (time is n-1) transmitted by the acceleration sensor receiving the flight vehicle, and the acceleration value of the acceleration measurement signal, in integral Alpha-Beta-Gama (integral α-β-γ) filtering process, which can generate a state prediction signal of a previous time (n-1), the value of the predicted state signal includes a predicting displacement, Predicting velocity and predicting acceleration; S2: Comparing the value of the measured signal from the previous time with the estimated displacement signal value, and obtaining the displacement error value of the current time (time n) (predicting displacement error); wherein the displacement error value may be weighted by the numerical value of the displacement measurement signal minus the estimated displacement signal value, but is not limited; S3: according to the displacement error value, the differential Alpha-Beta- Differential alpha-β-γ filtering process and generating a predictive correcting signal for at least one of the current time, the predicted corrected state signal including at least a predictive correcting displacement value Predictably correcting velocity and a predictive corrected acceleration signal value (predictabl) y correcting acceleration); S4: according to the estimated correction state signal of the current time and the predicted state signal of the previous time, combined to generate a state estimating signal of the current time, the estimated state signal The value contains at least the estimated speed of the current time The value of the estimated velocity, wherein the value of the estimated velocity signal may be a numerical weighting of the estimated velocity signal plus a value of the estimated correction velocity signal, but is not limited.

For different applications, the high-precision velocity estimation method of the present invention, further in step S4, the value of the estimated state signal may include a value of an estimated displacement signal of the current time, wherein the estimated displacement The value of the signal may be weighted by the numerical value of the estimated displacement signal plus the value of the estimated corrected displacement signal, but is not limited; or the value of the estimated state signal may include an estimated acceleration signal of the current time. The value of the estimated acceleration signal may be a numerical weighting of the estimated acceleration signal plus a value of the estimated corrected acceleration signal, but is not limited.

In order to more accurately estimate the value of the estimated speed of the flight vehicle, the high-precision speed estimation method of the present invention can further measure the acceleration value of the acceleration measurement signal obtained in step S1 first by gravity acceleration in Descartes. The gravity acceleration component on the coordinate XYZ axis is corrected. The correction method may use the acceleration value of the acceleration measurement signal to subtract the gravity acceleration component of the gravity acceleration on the XYZ axis, but is not limited.

According to another main object of the present invention, a high-precision speed estimating device is provided for a flying vehicle for receiving an acceleration measuring signal generated by an acceleration sensor of a flying vehicle and a displacement sensor Displacement measuring signal to generate an estimated state signal, the estimated state signal comprising at least an estimated speed signal of the flying vehicle; the speed estimating device comprising: a state predictor, a first computing unit, and a state corrector And a second operation unit; wherein the state predictor has an integral Alpha-beta-gamma (α-β-γ) filter processing function, and the acceleration measurement value of the acceleration measurement signal is subjected to calculation processing An estimated state signal is generated, the predicted state signal includes an estimated displacement signal, a predicted velocity signal, and a predicted acceleration signal; the estimated displacement signal can be a state vector having an estimated displacement in the XYZ direction. The estimated speed signal can be a state vector having a predicted velocity displacement value in the XYZ direction; the predicted acceleration signal can be a state vector having an estimated acceleration value in the XYZ direction; wherein the first operational unit is connected to The state predictor can receive the displacement measurement signal and the estimated displacement signal, and compare and output a displacement error signal; the displacement error signal can be a state vector The estimated displacement value of the XYZ direction; wherein the state corrector is connected to the first operation unit and the second operation unit, and has a differential Alpha-beta-gamma filter processing function, and can receive the displacement error signal Generating an estimated correction state signal, the predicted correction state signal includes at least an estimated corrected displacement signal, an estimated corrected velocity signal, and an estimated corrected acceleration signal; wherein the estimated corrected displacement signal is a vector having The estimated correction displacement value in the XYZ direction; the estimated correction speed signal can be a vector with an estimated corrected velocity displacement value in the XYZ direction; the estimated corrected acceleration signal can be a vector having an estimated corrected acceleration value in the XYZ direction; wherein the second computing unit is coupled to the state predictor and the state corrector to receive the estimated state signal and the estimated corrected state signal, The combination generates an estimated status signal, the estimated status signal including an estimated speed signal.

For different applications, the high-precision speed estimating device of the present invention, wherein the estimated state signal generated by the second computing unit, may further comprise an estimated displacement signal or an estimated acceleration signal.

In order to more accurately estimate the value of the estimated speed of the flying vehicle, the high-precision speed estimating device of the present invention further includes an acceleration measuring signal correction function for accelerating the acceleration measuring signal by gravity acceleration The value is corrected for the gravitational acceleration component on the three axes of the Cartesian coordinates XYZ on the flight carrier.

As described above, the high-precision speed estimation method and apparatus thereof according to the present invention can have the following advantages:

(1) The high-precision speed estimation method and apparatus of the present invention can be measured by the acceleration value of the acceleration signal measured by the UAV acceleration sensor and the displacement sensor of the UAV flight vehicle. The displacement value of the displacement signal is calculated by the integral α-β-γ filtering process and the differential α-β-γ filtering process, and the estimated state signal and the estimated correction state signal can be obtained, and the combination can generate high precision. Estimated speed values; this allows for precise manipulation of the UAV, improving the shortcomings of prior art techniques that cannot be achieved with the Kalman filter.

(2) The high-precision speed estimation method and apparatus of the present invention can measure the acceleration of the acceleration signal measured by the UAV acceleration sensor The value is subtracted from the gravitational acceleration component of the gravity acceleration value on the XYZ three axes of the UAV, and corrected by the integral α-β-γ filtering process to obtain an accurate predicted state signal; thereby improving the prior art. Consider the shortcomings of the effects of gravity acceleration.

Please refer to FIG. 1 , which is a block diagram of the high-precision speed estimating device of the present invention. In the figure, the high-precision velocity estimating device includes a state predictor 21, a state corrector 23, a first arithmetic unit 22, and a second arithmetic unit 24. The state predictor 21 receives the acceleration measurement signal generated by the acceleration sensor 11 , and the acceleration measurement signal has a value of the acceleration measurement. In a specific application, the acceleration sensor 11 of the flying vehicle can adopt a one-dimensional acceleration sensor or a three-dimensional inertial measurement unit (IMU) (such as the inertia amount of FIG. 6). The measuring unit 31); when the acceleration sensor 11 used by the flying vehicle is a one-dimensional acceleration sensor, the value of the acceleration measurement is the acceleration value a ₀ in the X direction at the current sampling time; The acceleration value a ₀ ( n -1) of n -1). If the acceleration sensor 11 used by the flying vehicle is a three-dimensional inertial measurement unit (IMU), the value of the acceleration measurement is the acceleration value a ₀ ( n -1) vector at the sampling time of the previous time (n-1). The a ₀ ( n -1 ) vector has an acceleration component of XYZ; for ease of explanation, the subsequent numerical symbols are numerical vectors having XYZ direction components, and the values of the displacement, the velocity, and the acceleration are respectively The numerical vector representing the displacement signal, the numerical vector of the velocity signal, and the numerical vector of the acceleration signal are simple and convenient.

The state predictor 21 has an integral Alpha-beta-gamma (α-β-γ) filtering function, which can calculate the value a ₀ ( n -1) of the acceleration measurement signal to generate a previous time ( The estimated state signal of time n-1), the estimated state signal includes an estimated displacement signal, an estimated speed signal, and an estimated acceleration signal; the estimated displacement signal value d _P ( n -1), an estimate The value of the velocity signal ν _P ( n -1) and the value of the predicted acceleration signal a _P ( n -1), with the vector set as the value of the predicted state signal X _P ( n -1) (for the state vector, state vector ), as shown in equation (1): X _P ( n -1) = [ d _p ( n -1) ν _P ( n -1) a _p ( n -1)] ^T (1)

Where X _P ( n -1) is the state vector of the value of the predicted state signal of the previous time; d _P ( n -1) is the value of the estimated displacement signal of the previous time; ν _P ( n -1) is the previous The value of the estimated velocity signal for time; a _P ( n -1) is the value of the estimated acceleration signal for the previous time.

The first operation unit 22 can receive the value d _P ( n -1) of the estimated displacement signal of the previous time generated by the state predictor 21, and the previous time generated by the displacement sensor 12 of the flight vehicle (n a displacement measurement signal of -1) having a value d ₀ ( n -1) of the displacement measurement signal. In a specific application, the displacement sensor 12 of the flight vehicle can sense the position of the flight vehicle and generate a displacement measurement signal from the position; the position can be a coordinate coordinate signal transmitted by the ground or a coordinate generated by the global positioning system. The signal is generated and is not limited. Similarly, the value d ₀ ( n -1) of the displacement measurement can be one-dimensional or multi-dimensional, and is still represented by a vector. The value X ₀ ( n -1) of the sensing state signal of the previous time, including the value d ₀ ( n -1) of the displacement measurement, the value of the velocity measurement, and the value of the acceleration measurement signal a ₀ ( n -1 ), since the flying vehicle is not in contact with the ground, the speed of the flying vehicle cannot be sensed by other devices, so the value d ₀ ( n -1 ) of the displacement measuring signal and the value of the acceleration measuring signal a ₀ ( n ) -1) A value combined into a sensing state signal, as shown in equation (2): X ₀ ( n -1) = [ d ₀ ( n -1) 0 a ₀ ( n -1)] ^T (2)

Where X ₀ ( n -1) is the state vector of the value of the sensing state signal of the previous time; d ₀ ( n -1) is the value of the displacement measurement of the previous time; a ₀ ( n -1) is the previous time The value of the acceleration measurement signal.

When the first operation unit 22 can receive the value d _P ( n -1) of the estimated displacement signal of the previous time and the value d ₀ ( n -1) of the previous time displacement measurement signal, the difference between the two values can be calculated and compared. And outputting a displacement error signal of the current time; likewise, the value Δ d ( n ) of the displacement error signal may be a vector having an estimated displacement value in the XYZ direction; and expressed as a state vector as Δ X ( n )=[ Δ d ( n )0 0] ^T . When comparing the difference between the two values, weighting subtraction comparison or simple subtraction can be used, which is not limited. For the sake of clarity, in Fig. 1, the value d ₀ ( n -1) of the simple displacement measurement signal is subtracted from the value d _P ( n -1) of the estimated displacement signal to generate a displacement error signal. The value Δ d ( n ) is as shown in the formula (3): Δ d ( n ) = d ₀ ( n -1) - d _p ( n -1) (3)

Where Δ d ( n ) is the value of the displacement error signal at the current time; d ₀ ( n -1) is the value of the displacement measurement signal of the previous time; d _P ( n -1) is the estimated displacement signal of the previous time The value.

The state corrector 23 has a differential Alpha-beta-gamma filter processing function, and can receive the value of the displacement error signal (indicated by the state vector Δ X ( n )) to generate an estimated correction state signal, and estimate the corrected state signal. The value is F. Δ X ( n ), where F is a gain matrix, which is calculated by differential Alpha-beta-gamma filtering. Estimated correction status signal F. △ X ( n ) includes an estimated corrected displacement signal, an estimated corrected speed signal, and an estimated corrected acceleration signal; wherein, the vector of the corrected corrected displacement signal has an estimated corrected displacement value in the XYZ direction; the estimated corrected speed signal It can be a vector with an estimated corrected velocity displacement value in the XYZ direction; the estimated corrected acceleration signal can be a vector with an estimated corrected acceleration value in the XYZ direction.

The second operation unit 24 can receive the predicted state signal of the previous time output by the state predictor 21 and the estimated correction state signal output by the state corrector 23, and compare the value of the predicted state signal X _P ( n -1) with the correcting the estimated state of the signal values F. Δ X ( n ) is combined to generate an estimated state signal for the current time; wherein the estimated state signal value X ( n ) contains the value of the estimated velocity signal, or further includes the estimated displacement signal value and estimate The value of the acceleration signal; the value X ( n ) of the estimated state signal is an accurate estimate of the estimated speed of the flight vehicle, or further includes an accurate estimate of the estimated displacement of the flight vehicle, the estimated acceleration. The method of combining the predicted state signal with the estimated correction state signal may adopt different methods, such as simple addition calculation or weighted addition, which are not limited; for clear description, in the first figure, in the simple The value of the estimated speed signal plus the value of the estimated corrected speed signal to produce an accurate estimate of the speed signal value, as shown in equation (4): X ( n ) = X _P ( n -1) + △ X _P ( n )= X _P ( n -1) + F . △ X ( n ) (4)

Where X ( n ) is the value of the estimated state signal for the current time; X _P ( n -1) is the value of the predicted state signal of the previous time; Δ X _P ( n ) = F . △ X ( n ) is the value of the estimated correction state signal of the current time; F is the gain matrix calculated by the differential Alpha-beta-gamma filter processing function; Δ X ( n ) is the displacement error signal of the current time The state vector of the value.

Since the flight vehicles at different times may not necessarily be parallel to the sea level when flying, as shown in Figure 7, the flight vehicle flies at a pitch angle θ, and the gravitational acceleration g is on the three axes of the Cartesian coordinate XYZ of the flight vehicle. The gravity acceleration component is generated; in order to enable the state predictor 21 to accurately generate the predicted state signal, the state predictor 21 has an acceleration measurement signal correction function, and the acceleration measurement signal output by the acceleration sensor 11 can be gravity The acceleration value is corrected for the gravitational acceleration component on the three axes of the Cartesian coordinates on the flight carrier, as corrected on equation X (5) on the X-axis of the flight vehicle:

Where a ₀ ( n -1) is the value of the acceleration measurement signal corrected in the previous time; ( n -1) is the value of the acceleration measurement signal output by the previous time acceleration sensor 11; g is the gravitational acceleration and θ( n -1) is the pitch angle of the previous time flight vehicle.

The global positioning system 32 has different application systems. For a preferred application embodiment, the global positioning system 32 can be a differential global positioning system (DGPS); the DGPS system uses nearby known reference coordinate points to correct the global positioning system. The error, and then add this real time error value to the consideration of its own coordinate operation, in order to obtain the coordinates of the coordinates of the flying vehicle. The global positioning system 32 in this embodiment is implemented using a differential global positioning system DGPS.

For different flight vehicle configurations, Global Positioning System 32 can also be Wide Area Augmentation System GPS (WAAS-DGPS). WAAS-DGPS can correct for ionospheric interference, incorrect timing control, and satellite orbits. GPS signal error caused by factors such as errors to obtain a more accurate coordinate position of the flying vehicle.

A high-precision speed estimation method for estimating an estimated speed of a flying vehicle using the high-precision speed estimating device of the present invention is as follows: S0: an acceleration sensor 11 of the flying vehicle can accelerate the flying vehicle The measurement signal is output to the state predictor 21; the displacement sensor 12 of the flight vehicle can output the displacement measurement signal of the flight vehicle to the first operation unit 22; S1: the state predictor 21 receives the acceleration of the flight vehicle The acceleration measurement signal of the previous time transmitted by the sensor 11 has the value a ₀ ( n -1) of the acceleration measurement signal processed by the integral Alpha-beta-gamma filter, and the predicted state of the previous time is generated. Signal, the state vector X _P ( n -1) of the value of the predicted state signal, including the estimated displacement value d _P ( n -1), the estimated velocity value ν _P ( n -1), and the estimated acceleration value a _P ( n -1); S2: The current time (time is n) is obtained by comparing the value d ₀ ( n -1) of the displacement measurement signal with the estimated displacement signal value d _P ( n -1) according to the previous time. displacement error value △ d (n); mode displacement error value △ d (n) may be employed formula (3) by the displacement The measured signal value d ₀ (n -1) signal by subtracting the estimated value of the displacement _{d P (n -1); S3} : displacement of the time of the error value △ d (n), the state vector is expressed as △ X (n After the differential Alpha-beta-gamma filter processing, the gain matrix F is calculated; from F. △ X ( n ) calculates the value of the estimated correction state signal obtained at the current time, and the estimated correction state signal includes the estimated corrected displacement signal value, the estimated corrected speed signal value, and the estimated corrected acceleration signal value for correcting Estimating the value of the status signal; S4: Correcting the state vector F of the value of the status signal based on the current time estimate. Δ X ( n ) and the state vector X _P ( n -1) of the value of the predicted state signal of the previous time, the state vector X ( n ) of the value of the estimated state signal of the current time is combined by the formula (4) The value of the estimated status signal contains the value of the estimated speed signal for the current time.

For different applications, the high-precision speed estimation method of the present invention further includes, in step S4, the value of the estimated state signal may include a value of the estimated displacement signal of the current time, wherein the value of the estimated displacement signal may be The value of the estimated displacement signal is added to the value of the estimated corrected displacement signal; or the value of the estimated state signal may include the value of the estimated acceleration signal at the current time, wherein the value of the estimated acceleration signal may be Estimate the value of the acceleration signal plus the estimated correction The value of the acceleration signal.

In order to more accurately estimate the value of the estimated speed of the flight vehicle, the high-precision speed estimation method of the present invention further measures the acceleration value a ₀ ( n -1) of the acceleration measurement signal obtained in step S1, First, the gravity acceleration component on the Cartesian coordinate XYZ axis is corrected by gravity acceleration; wherein the correction method can be calculated as Equation (5).

Please refer to FIG. 6 , which is a block diagram of a specific embodiment of the high-precision speed estimating device of the present invention. The embodiment is used for an unmanned aircraft (UAV). As shown in the figure, an unmanned aircraft is equipped with an inertia amount. The measurement unit (IMU) 31 and a global positioning system (GPS) 32 are respectively embodied in the acceleration sensor 11 and the displacement sensor 12 in FIG. 1; the inertia measurement unit 31 is based on the sampling time. The UAV XYZ three-axis acceleration measurement signal can be measured, and the value of the acceleration measurement signal is ( n -1); the global positioning system 32 converts the coordinate into a displacement measurement signal according to the sampling time, and the value of the displacement measurement signal is d ₀ ( n -1).

The high-precision velocity estimating device includes: a state state predicting filter 33, a displacement signal assembler 34, a state correcting filter 36, and an estimated signal combiner (estimation) Signal assembler) 35; wherein the state prediction filter 33 is provided with an integral Alpha-beta-gamma (α-β-γ) filter, and the integral α-β-γ filter can be used at time n- At 1 o'clock, the measured value of the acceleration signal output by the inertial measurement unit 31 ( n -1), first corrected by the gravity acceleration component via equation (5), and then transmitted to the integral α-β-γ filter calculation process to generate an estimated state signal, the state of the predicted state signal The vector is X _P ( n -1), including the value d _P ( n -1) of the estimated displacement signal, the value of the estimated velocity signal ν _P ( n -1), and the value of the predicted acceleration signal a _P ( n -1); Here, the inertial measurement unit 31 installed in the UAV is a three-dimensional measurement unit that can output an acceleration signal of the XYZ axis, so the value of the displacement signal is estimated to be d _P ( n -1), and an estimate is made. value of the speed signal ν _P (n -1) and the estimated value of the acceleration signal a _P (n -1) are the XYZ components of the set of vectors.

The displacement signal combiner 34 is a subtractor that subtracts the value d ₀ ( n -1) of the displacement measurement signal from the value d _P ( n -1) of the estimated displacement signal to generate a value Δ d of the displacement error signal. ( n ), that is, as shown in the equation (3), if represented by a state vector, it is Δ X ( n )=[Δ d ( n )0 0] ^T .

State correction filter 36 is provided with a derivative of formula alpha - beta - Gama filter, the value may △ X (n) to generate a displacement error signal estimate correction state signal, correcting the estimated value of the status signal F. △ X (n), where F is the gain matrix based differential expression of alpha - generating Gama filter calculation, the estimated value of the state of the correction signal F - beta. Δ X ( n ) can be used to correct the state vector X _P ( n -1) of the predicted state signal output by the state prediction filter 33.

The estimated signal combiner 35 is an adder that can estimate the value of the predicted state signal X _P ( n -1) from the value of the predicted corrected state signal F . Δ X ( n ), added to produce a value X ( n ) of the estimated state signal, as shown in equation (4). In this embodiment, the estimated state signal includes an estimated displacement signal, an estimated velocity signal, and an estimated acceleration signal.

Where d ( n ) is the value of the estimated displacement signal, ν( n ) is the value of the estimated velocity signal and a ( n ) is the value of the estimated acceleration signal.

FIG. 8 illustrates a step of applying the high-precision velocity estimation method of the present invention to the steps of the present embodiment, as follows: S1: calculating a state vector of the value of the estimated state signal; and including the following steps: S11: acceleration measurement signal Numerical value ( n -1), first corrected by gravity acceleration component via equation (5); S12: measured value a ₀ ( n -1) of the corrected acceleration signal, calculated by integral α-β-γ filter S13: generating an estimated state signal of the previous time, the state vector X _P ( n -1) of the value of the predicted state signal, including the value d _P ( n -1) of the estimated displacement signal, and the estimated speed signal The value ν _P ( n -1) and the value of the predicted acceleration signal a _P ( n -1); S2: Calculate the state vector of the displacement error value, including the following steps: S21: measuring the signal by the displacement of the previous time The value d ₀ ( n -1) and the value d _P ( n -1) of the estimated displacement signal are subtracted by the equation (3); S22: the displacement error value Δ d ( n ) is generated, which is represented by the state vector as Δ X ( n ); S3: Calculate the value of the estimated correction state signal, including the following steps: S31: Displacement error value state vector △ X ( n ) is processed by differential Alpha-beta-gamma filter, and then calculated Output gain matrix F ; S32: Calculate the value of the estimated correction state signal obtained at the current time ΔX _P (n)= F . △ X ( n ); the estimated correction state signal includes an estimated corrected displacement signal value, a predicted corrected speed signal value, and a predicted corrected acceleration signal value; S4: calculating a value of the estimated state signal, comprising the following steps: S41: The state vector Δ X _P ( n ) according to the value of the estimated correction state signal and the state vector X _P ( n -1) of the value of the estimated state signal are added by the formula (4); S42: generating the estimation A state vector X ( n ) of the value of the state signal; a value d ( n ) containing the estimated displacement signal, a value ν( n ) of the estimated velocity signal, and a value a ( n ) of the estimated acceleration signal.

Please refer to FIGS. 2 and 3, which illustrate the value Δ X ( n ) of the displacement error signal outputted by the displacement signal combiner 34 (step S22) and the value of the estimated correction state signal output by the state correction filter 36. △X _P (n)= F . Δ X ( n ) (step S32), the state vector X ( n ) of the value of the estimated state signal output by the estimated signal combiner 35 (step S42), the process of changing at different times; in the second figure, n The state of -1 time, the value of the estimated state X ( n -1) via the estimated correction state ΔX _P (n-1) = F . Δ X ( n -1) is combined with the value X _P ( n ) of the estimated state of n time to become the estimated value X ( n ); at n time, the estimated value X ( n ) is estimated Correct the value of the status signal △X _P (n)= F . Δ X ( n ) is combined with the value X _P ( n +1) of the estimated state of n time to become the value X ( n +1) of the estimated state; In Fig. 3, the state of the n-1 time is determined by the displacement error value Δ X ( n -1) via the value F of the estimated correction state. Δ X ( n -1) is combined with the value Δ X _P ( n ) of the estimated correction state with n time; at time n, the value F of the corrected correction state is obtained from the displacement error value Δ X ( n ). Δ X ( n ) is combined into a value Δ X _P ( n +1) of the estimated correction state with n+1 time; the following analogy; thus the displacement error value Δ X ( n ) gradually decreases toward the time by the recursion of time The displacement error, or the displacement error, is getting smaller and smaller, and the estimated velocity value is closer to the true velocity value.

This can also be illustrated by FIG. 4 and FIG. 5, which are estimated speed values and true speed values output by the estimated signal combiner 35 (which is equivalent to the second arithmetic unit of the high-precision speed estimating device of the present invention). And the first comparison map and the second comparison graph of the speed value of the noise, it can be seen that the high-precision speed estimation method and device of the embodiment can effectively improve the noise, and the acquisition and the real The result of similar speed.

The above is intended to be illustrative only and not limiting. Any equivalent modifications or alterations to the spirit and scope of the invention are intended to be included in the scope of the appended claims.

11‧‧‧Acceleration measurer

12‧‧‧displacement measurer

21‧‧‧ state predictor

22‧‧‧first computing unit

23‧‧‧state corrector

24‧‧‧second computing unit

31‧‧‧Inertial Measurement Unit (IMU)

32‧‧‧Global Positioning System (GPS)

33‧‧‧previous state predicting filter

34‧‧‧displacement signal assembler

35‧‧‧ Estimation signal assembler

36‧‧‧state correcting filter

S11~S42‧‧‧Steps

Figure 1 is a block diagram of the high-precision speed estimating device of the present invention; 2 is a flow chart of the state of the predictor of the state predictor of the present invention and the state of the estimated state vector at different times; FIG. 3 is a state corrective vector of the state corrector of the present invention and the estimated state correction The change process of the vector at different times; the fourth figure is the first comparison chart of the speed value, the real speed value and the speed value of the noise signal of the estimated signal combiner of the embodiment of the invention; A second comparison diagram of the speed value, the real speed value, and the speed value of the noise signal outputted by the estimated signal combiner according to the embodiment of the present invention; FIG. 6 is a high precision speed estimating apparatus embodiment of the present invention. Block diagram; Figure 7 is a schematic diagram of the force of the flight vehicle during flight; and Figure 8 is a flow chart of the implementation steps of the high-precision speed estimation method of the present invention.

11‧‧‧Acceleration measurer

12‧‧‧displacement measurer

21‧‧‧ state predictor (previous state estimator)

22‧‧‧first computing unit

23‧‧‧corrector

24‧‧‧second computing unit

Claims (10)

A high-precision speed estimating device is used for a flying vehicle for receiving an acceleration measuring signal generated by an acceleration sensor of a flying vehicle and a displacement measuring signal generated by a displacement sensor to generate An estimated state signal; comprising: a state predictor, a first arithmetic unit, a state corrector, and a second arithmetic unit; wherein the state predictor has integral Alpha-beta-gamma ( The α-β-γ) filtering processing function can receive the acceleration measurement signal to generate an estimated state signal, and the estimated state signal includes at least an estimated displacement signal, a predicted speed signal, and a predicted acceleration signal; The first operation unit is connected to the state predictor and has a displacement comparison function, and can receive the displacement measurement signal and the estimated displacement signal, and compare and output a displacement error signal; wherein the state corrector Connected to the first operation unit and the second operation unit, having a differential Alpha-beta-gamma filter processing function, and receiving the displacement error signal to generate an estimated correction state signal, The estimated correction state signal includes at least an estimated corrected displacement signal, an estimated corrected speed signal, and an estimated corrected acceleration signal; wherein the second computing unit is coupled to the state predictor and the state corrector, Receiving the estimated state signal and the estimated correction state signal, combined to generate an estimated state signal, the estimated state signal comprising at least one estimated speed signal. The high-precision speed estimating device according to claim 1, wherein the state predictor has an acceleration measuring signal correcting function, and the acceleration measuring signal can be a Cartesian coordinate on the flying vehicle with a gravity acceleration value. The gravitational acceleration components on the three axes are corrected. The high-precision speed estimating device according to claim 1, wherein the estimated state signal generated by the second computing unit further comprises an estimated displacement signal. The high-precision speed estimating device according to claim 1, wherein the estimated state signal generated by the second computing unit further comprises an estimated acceleration signal. The speed estimating device according to claim 1, wherein the acceleration sensor of the flying vehicle is constituted by an inertial measuring unit, and the state predictor can receive the inertial measuring unit. The acceleration measurement signal. The high-precision speed estimating device according to claim 1, wherein the displacement sensor of the flying vehicle is constituted by a positioning positioning system, and the first computing unit can receive the coordinate positioning system. The displacement measurement signal. The high-precision speed estimating device according to claim 6, wherein the coordinate positioning system of the flying vehicle is selected from the group consisting of a differential global positioning system and a wide-area enhanced differential global positioning system. A high-precision speed estimation method using a speed estimation device of the first application scope of the patent, one obtained from previous time measurement The displacement measurement value and the acceleration measurement value are used to estimate the value of an estimated speed signal at the current time; the speed estimation method comprises the following steps: S1: integrating the value of the acceleration measurement signal according to one of the previous times to integrate Alpha-beta-gamma filter processing, and generating a value of one of the previous time state signals, the value of the predicted state signal includes at least a value of the estimated displacement signal, a value of the estimated speed signal, and a value of the estimated acceleration signal; S2: comparing the value of the displacement measurement signal according to the previous time with the value of the estimated displacement signal to obtain a displacement error value of the current time; S3: according to the displacement error value, The differential Alpha-Beta-Gama filter process generates a value of the estimated correction state signal for one of the current times, and the value of the estimated correction state signal includes at least a value of the estimated corrected displacement signal, and an estimated correction The value of the speed signal and the value of an estimated corrected acceleration signal; S4: the estimated state signal according to the estimate of the current time and the predicted state signal of the previous time, Generating one signal present time estimated state of engagement, the value of the estimated state signal including at least the numerical value of the estimated velocity signal present time. The high-precision speed estimation method according to claim 8, wherein the value of the acceleration measurement signal at the previous time of step S1 further passes through a gravity acceleration on the XYZ axis of the Cartesian coordinate on the flying carrier. The value component is corrected. The high-precision speed estimation method according to Item 8 of the patent application, wherein the value of the estimated state signal of step S4 further includes one of a value of the estimated displacement signal at the current time and a value of the estimated acceleration signal. Or a combination thereof.