Speed smoothing filtering method and system based on adaptive PI filtering

Abstract

The invention discloses a speed smoothing filtering method and system based on self-adaptive PI filtering, and solves the problems that in the prior art, when a Beidou / GNSS chip is used for navigation positioning, PVT is used for solving the speed, the filtering effect is poor, errors are large, and the navigation precision is low. The method comprises the steps that the original speed and speed variance of PVT solving at the current moment are obtained; calculating a speed difference value between the current moment and the previous moment, and judging a current motion state according to the speed difference value; adaptively adjusting the bandwidth of an adaptive PI filter according to the speed variance and the current motion state; and calculating a P coefficient and an I coefficient of the PI filter by using the adjusted bandwidth, and carrying out smooth filtering on the original speed to obtain a filtered speed. The bandwidth of the filter is adaptively adjusted according to the speed acceleration and deceleration conditions and the speed variance, effective smooth filtering of the PVT resolving speed is realized, and the navigation precision and reliability are improved.

Description

Technical Field

[0001] The present invention relates to the field of satellite navigation technology, and in particular to a velocity smoothing filtering method and system based on adaptive PI filtering. Background Art

[0002] With the development of economy and technology and the improvement of people's living standards, navigation technology has been widely used in people's production and daily life, bringing endless convenience to people's work and life. Therefore, ensuring the accuracy and real-time performance of the navigation process has become a key research focus. The main purpose of GNSS navigation receiver calculation is to determine the receiver's position (position), velocity (velocity), and time (time). Currently, when using Beidou / GNSS chips for navigation and positioning, the chips use PVT calculations to obtain important information such as position, velocity, and time.

[0003] For example, the Chinese Patent Office published a patent on January 21, 2022: CN113960918A, a single-line timing and timekeeping method based on the global satellite navigation system GNSS. The PVT solution is completed through the navigation positioning solution module of the GNSS receiver to obtain the position, speed and time information of the GNSS receiver. However, in actual applications, affected by factors such as signal interference and multipath effects, the speed results obtained by PVT solution often have abnormal jumps and large fluctuation errors, which seriously reduce the accuracy and reliability of navigation. The traditional speed filtering method uses fixed filtering parameters and cannot be adaptively adjusted according to the actual speed changes and speed variance. The filtering effect is poor in different motion scenarios, and it is difficult to balance the filtering effect and response speed. It is easy to have problems of excessive or insufficient filtering. Summary of the Invention

[0004] The purpose of the present invention is to solve the problem in the prior art that when using Beidou / GNSS chips for navigation and positioning, the PVT solution speed is used, the filtering effect is poor, the error is large, and the navigation accuracy is low. A speed smoothing filtering method and system based on adaptive PI filtering are provided. By combining the original speed and speed variance, an adaptive PI filter is introduced, and the filter bandwidth is adaptively adjusted according to the speed acceleration and deceleration conditions and the speed variance, so as to achieve effective smoothing filtering of the PVT solution speed and improve the navigation accuracy and reliability.

[0005] In order to achieve the above object, the present invention adopts the following technical solutions: A velocity smoothing filtering method based on adaptive PI filtering comprises the following steps: S1: Get the original speed and speed variance at the current moment obtained by PVT solution; S2: Calculate the speed difference between the current moment and the previous moment, and determine the current motion state based on the speed difference; S3: Adaptively adjust the bandwidth of the adaptive PI filter according to the velocity variance and the current motion state; S4: Calculate the P coefficient and I coefficient of the adaptive PI filter using the adjusted bandwidth, and perform smoothing filtering on the original speed to obtain the filtered speed.

[0006] The "P" in adaptive PI stands for proportion, and the "I" stands for integral. The method of the present invention combines the original velocity with the velocity variance to introduce an adaptive PI filter. This filter bandwidth is adaptively adjusted based on the acceleration and deceleration conditions and the velocity variance, achieving effective smoothing of the PVT solution velocity. An asymmetric filtering strategy is employed, enhancing filtering during acceleration and reducing filtering during deceleration, thereby improving navigation accuracy and reliability.

[0007] Preferably, performing smooth filtering on the original speed to obtain the filtered speed comprises: Calculate the P coefficient and I coefficient of the PI filter; calculate the error between the current original speed and the filtered speed at the previous moment; use the I coefficient and speed error to calculate the integral component of the speed filter at the current moment and update the integral term; use the filtered speed and P coefficient at the previous moment, as well as the updated integral term, to calculate the filtered speed at the current moment.

[0008] Preferably, S3 includes: the bandwidth of the adaptive PI filter is the product of the speed variance bandwidth component and the acceleration / deceleration state bandwidth component, and the speed variance bandwidth component is adjusted according to the speed variance: the larger the speed variance, the smaller the speed variance bandwidth component; and the acceleration / deceleration state bandwidth component is adjusted according to the current motion state: the acceleration / deceleration state bandwidth component is small during the acceleration process, and the acceleration / deceleration state bandwidth component is large during the deceleration process.

[0009] Preferably, adjusting the acceleration / deceleration state bandwidth component according to the current motion state includes: if it is an acceleration state, the acceleration / deceleration state bandwidth component is the minimum value of the preset acceleration / deceleration state bandwidth component; if it is a deceleration state, the acceleration / deceleration state bandwidth component is the maximum value of the preset acceleration / deceleration state bandwidth component; if it is a uniform speed state, the acceleration / deceleration state bandwidth component is the median value of the preset acceleration / deceleration state bandwidth component.

[0010] Preferably, the P coefficient is k times the bandwidth of the updated adaptive PI filter, k is a fixed coefficient, and the I coefficient is the square of the updated adaptive PI filter bandwidth.

[0011] Preferably, the velocity variance bandwidth component is the product of the maximum value of the velocity variance bandwidth component and a scaling factor, and the scaling factor is a monotonically decreasing function of the velocity variance.

[0012] Preferably, the S1 further comprises: initializing relevant parameters of the adaptive PI filter, and simultaneously initializing the integral term and the speed after filtering at the previous moment.

[0013] Preferably, the filtered speed at the previous moment is obtained, the product of the P coefficient and the error at the current moment is obtained, and the sum of the products of the I coefficient and the error at each moment from 0 to the current moment is obtained. The filtered speed is the sum of the three.

[0014] Preferably, in S2, if the speed difference is greater than 0, it is an acceleration state; if the speed difference is less than 0, it is a deceleration state; if the speed difference is 0, it is a uniform speed state.

[0015] A speed smoothing filter system based on adaptive PI filtering, comprising: The data acquisition module obtains the original speed and speed variance at the current moment obtained by PVT solution; The acceleration and deceleration judgment module calculates the speed difference between the current moment and the previous moment, and determines the current motion state based on the speed difference; The bandwidth adjustment module adaptively adjusts the bandwidth of the adaptive PI filter according to the velocity variance and the current motion state; The adaptive PI filter module calculates the P coefficient and I coefficient of the adaptive PI filter using the adjusted bandwidth, and performs smoothing filtering on the original speed to obtain the filtered speed.

[0016] Therefore, the present invention has the following beneficial effects: 1. The filter bandwidth is adaptively adjusted based on the speed variance and acceleration / deceleration status. The filter parameters can be adjusted in real time according to different motion scenarios to improve the filtering effect.

[0017] 2. Adopt an asymmetric filtering strategy. When accelerating, reduce the bandwidth to strengthen filtering, effectively filtering out abnormal speed jumps; when decelerating, increase the bandwidth and reduce filtering, ensuring system response speed and improving navigation accuracy and reliability. BRIEF DESCRIPTION OF THE DRAWINGS

[0018] Figure 1 This is a flow chart of the overall steps of the speed smoothing filtering method based on adaptive PI filtering in Example 1.

[0019] Figure 2 This is a structural block diagram of the speed smoothing filter system based on adaptive PI filtering in Example 2.

[0020] Figure 3 This is a structural block diagram of the adaptive PI filter bandwidth adjustment in Example 2.

[0021] In the figure: 1. Data acquisition module; 2. Acceleration and deceleration judgment module; 3. Bandwidth adjustment module; 4. Adaptive PI filtering module; 5. Result output module. DETAILED DESCRIPTION

[0022] The present invention is further described in detail below with reference to the accompanying drawings and specific embodiments: Example 1: This embodiment provides a speed smoothing filtering method based on adaptive PI filtering, such as Figure 1 As shown in FIG, the operation process is as follows: step 1, obtaining the original speed and speed variance of the current moment of PVT solution; step 2, calculating the speed difference between the current moment and the previous moment, and judging the current motion state according to the speed difference; step 3, adaptively adjusting the bandwidth of the adaptive PI filter according to the speed variance and the current motion state; step 4, calculating the P coefficient and I coefficient of the adaptive PI filter using the adjusted bandwidth, and smoothing the original speed to obtain the filtered speed.

[0023] This embodiment provides a velocity smoothing method based on adaptive PI filtering. By combining the original velocity and velocity variance, an adaptive PI filter is introduced. The filter bandwidth is adaptively adjusted based on the acceleration and deceleration conditions and the velocity variance, achieving effective smoothing of the PVT solution velocity. An asymmetric filtering strategy is employed, strengthening the filtering during acceleration and reducing it during deceleration, thereby improving navigation accuracy and reliability.

[0024] The following further illustrates the technical solutions and technical effects of the present invention through specific examples and specific application scenarios. The following examples are intended to explain the present invention, but the present invention is not limited to the following examples.

[0025] Step 1: Get the original speed and speed variance of the current moment of PVT solution.

[0026] Initialize the adaptive PI filter related parameters, including the maximum value of the speed variance bandwidth component, the scaling factor, the minimum value of the acceleration / deceleration state bandwidth component, the median value of the acceleration / deceleration state bandwidth component, the maximum value of the acceleration / deceleration state bandwidth component, and the fixed coefficient k. At the same time, initialize the integral term and the filtered speed at the previous moment.

[0027] The basic principle of the PVT algorithm is to measure the distance difference between the receiver and each satellite through the received satellite navigation signal, and use this distance information to calculate the receiver's position, speed and time.

[0028] This embodiment first uses the PVT algorithm to obtain the current raw velocity and velocity variance from the PVT calculation module of the BeiDou / GNSS chip. The receiver's velocity can be calculated based on the rate of change of the distances received from multiple satellites. The basic principle of velocity calculation is to estimate velocity using the difference between two positions and the time difference.

[0029] Affected by factors such as signal interference and multipath, the velocity results obtained from PVT calculations often exhibit abnormal jumps and large fluctuation errors, severely reducing navigation accuracy and reliability. This embodiment primarily provides effective smoothing filtering for the PVT calculated velocity. By employing an asymmetric filtering strategy, filtering is strengthened during acceleration and reduced during deceleration, improving navigation accuracy and reliability.

[0030] Step 2: Calculate the speed difference between the current moment and the previous moment, and determine the current motion state based on the speed difference.

[0031] Calculate the speed difference between the original speed at the current moment and the original speed at the previous moment. If the speed difference is greater than 0, it is an acceleration state; if the speed difference is less than 0, it is a deceleration state; if the speed difference is 0, it is a constant speed state.

[0032] Step 3: Adaptively adjust the bandwidth of the adaptive PI filter according to the velocity variance and the current motion state.

[0033] In this embodiment, the bandwidth of the adaptive PI filter consists of two parts, namely, a speed variance bandwidth component and an acceleration / deceleration state bandwidth component. The speed variance component bandwidth is a bandwidth component related to the speed variance, and the acceleration / deceleration state bandwidth component is a bandwidth component related to the acceleration / deceleration state.

[0034] Specifically, the bandwidth of the adaptive PI filter is the product of the speed variance bandwidth component and the acceleration / deceleration state bandwidth component.

[0035] When adjusting the bandwidth of the adaptive PI filter, it includes: (1) Adjust the speed variance bandwidth component according to the speed variance: the larger the speed variance, the smaller the speed variance bandwidth component.

[0036] Specifically, the velocity variance bandwidth component is equal to the product of the maximum value of the velocity variance bandwidth component (which can be set to a fixed value, such as 1.0) and the scaling factor. The scaling factor is a monotonically decreasing function of the velocity variance; the larger the velocity variance, the smaller the scaling factor.

[0037] The monotonically decreasing function can be implemented by a lookup table. For example, when the speed variance is less than a certain value (0.2 in this embodiment), the scaling factor reaches its maximum value of 1.0; when the speed variance is greater than or equal to 0.2 and less than 0.4, the scaling factor is equal to 0.8; when the speed variance is greater than or equal to 0.4 and less than 0.6, the scaling factor is equal to 0.5, and so on.

[0038] (2) Adjust the acceleration / deceleration state bandwidth component according to the current motion state: the acceleration / deceleration state bandwidth component is small during the acceleration process, and the acceleration / deceleration state bandwidth component is large during the deceleration process.

[0039] Specifically, adjusting the acceleration / deceleration state bandwidth component according to the current motion state includes: If it is in the acceleration state, the acceleration / deceleration state bandwidth component is the minimum value of the preset acceleration / deceleration state bandwidth component; If it is in the deceleration state, the acceleration / deceleration state bandwidth component is the maximum value of the preset acceleration / deceleration state bandwidth component; If the vehicle is in a uniform speed state, the acceleration / deceleration state bandwidth component is the median value of the preset acceleration / deceleration state bandwidth components.

[0040] The minimum value, maximum value, and median value of the acceleration / deceleration state bandwidth component are all preset fixed values. Their values ​​can be set arbitrarily, but they must meet the following conditions: the minimum value of the acceleration / deceleration state bandwidth component is less than the median value of the acceleration / deceleration state bandwidth component, and the median value of the acceleration / deceleration state bandwidth component is less than the maximum value of the acceleration / deceleration state bandwidth component.

[0041] Step 4: Use the adjusted bandwidth to calculate the P coefficient and I coefficient of the adaptive PI filter, and smooth the original speed to obtain the filtered speed.

[0042] The principle of an adaptive PI filter is as follows: the input signal passes through a parameter-adjustable digital filter to generate an output signal, which is then compared with the desired signal to form an error signal. The filter parameters are then adjusted using an adaptive algorithm to minimize the mean square value of the error signal. Adaptive filtering can automatically adjust the filter parameters at the current moment using the results of the filter parameters obtained at the previous moment to adapt to the unknown or time-varying statistical characteristics of the signal and noise, thereby achieving optimal filtering.

[0043] In this embodiment, the bandwidth of the adaptive PI filter after adjustment is used to adjust the original speed v at the current moment. raw (t) Perform adaptive PI smoothing filtering to obtain the filtered velocity v at the current moment filtered The specific process of (t) is: (1) Calculate the P coefficient and I coefficient of the PI filter.

[0044] The P coefficient is the product of the fixed coefficient and the bandwidth of the adjusted adaptive PI filter; and the I coefficient is the square of the bandwidth of the adjusted adaptive PI filter.

[0045] (2) Calculate the error signal e(t).

[0046] The error signal is the difference between the original speed at the current moment and the filtered speed at the previous moment.

[0047] (3) Update the integral term.

[0048] The updated value is the sum of the integral term at the previous moment and the product of the I coefficient and the error signal at the current moment. The integral term at the previous moment is the sum of the product of the I coefficient and the error signal from time 0 to the previous moment (that is, the error integral value accumulated from time 0 to the previous sampling moment).

[0049] (4) Calculate the filtering results.

[0050] The filtered speed at the current moment is the sum of the filtered speed at the previous moment, the updated integral term, and the product of the P coefficient and the error signal.

[0051] The filtered velocity at the current moment is output as the final velocity for subsequent navigation applications.

[0052] The velocity smoothing filtering method based on adaptive PI filtering provided in this embodiment can be implemented in a digital signal processing (DSP) module or a microcontroller (MCU) of a Beidou / GNSS chip.

[0053] The speed smoothing filtering method based on adaptive PI filtering provided in this embodiment adaptively adjusts the filter bandwidth according to the speed acceleration and deceleration conditions and speed variance, thereby achieving effective smoothing filtering of the PVT solution speed and improving navigation accuracy and reliability.

[0054] The speed smoothing filtering method based on adaptive PI filtering provided in this embodiment has the following advantages: (1) Adaptive adjustment: The filter bandwidth B_n is adaptively adjusted based on the speed variance and acceleration / deceleration state. The filter parameters can be adjusted in real time according to different motion scenes to improve the filtering effect.

[0055] (2) Asymmetric filtering: Adopting an asymmetric filtering strategy, the bandwidth is reduced to strengthen filtering during acceleration, effectively filtering out abnormal speed jumps; the bandwidth is increased to reduce filtering during deceleration, ensuring system response speed and improving navigation accuracy and reliability.

[0056] (3) Strong versatility: This method is applicable to Beidou / GNSS chips, has strong versatility and portability, and can be widely used in various navigation devices based on Beidou / GNSS chips.

[0057] Example 2: This embodiment provides a speed smoothing filtering system based on adaptive PI filtering, which is used to implement the speed smoothing filtering method based on adaptive PI filtering in the first embodiment.

[0058] Specifically, such as Figure 2 As shown, a speed smoothing filter system based on adaptive PI filtering includes: Data acquisition module 1, obtains the original speed and speed variance of the current moment of PVT solution; Acceleration / deceleration judgment module 2 calculates the speed difference between the current moment and the previous moment, and judges the current motion state based on the speed difference; Bandwidth adjustment module 3, adaptively adjusts the bandwidth of the adaptive PI filter according to the velocity variance and the current motion state; The adaptive PI filter module 4 calculates the P coefficient and the I coefficient of the adaptive PI filter using the adjusted bandwidth, and performs smoothing filtering on the original speed to obtain the filtered speed.

[0059] The result output module 5 converts the filtered speed v filtered (t) is output as the final velocity for subsequent navigation applications.

[0060] Specifically, such as Figure 2 As shown in the figure, the various modules of the filtering system are presented, including the data acquisition module, acceleration and deceleration judgment module, bandwidth adjustment module, adaptive PI filtering module and result output module, as well as the data flow and interaction relationship between the modules.

[0061] The data acquisition module is used to obtain the current original speed and speed variance from the PVT solution module of the Beidou / GNSS chip, and send the original speed to the acceleration and deceleration judgment module and the adaptive PI filter module, while sending the speed variance to the bandwidth adjustment module.

[0062] The acceleration / deceleration judgment module first judges the current motion state according to the original speed sent by the data acquisition module.

[0063] The specific judgment process is: Calculate the speed difference dv between the current speed and the previous speed, and determine whether the current motion state is acceleration, deceleration, or constant speed based on the positive or negative value of the speed difference dv.

[0064] If the speed difference dv is greater than 0, the current motion state is determined to be an acceleration state; if the speed difference dv is less than 0, the current motion state is determined to be a deceleration state; if the speed difference dv is equal to 0, the current motion state is determined to be a uniform speed state.

[0065] The acceleration / deceleration judgment module sends the current motion state obtained by judgment to the bandwidth adjustment module.

[0066] The bandwidth adjustment module is used to adjust the bandwidth of the adaptive PI filter.

[0067] The specific adjustment process is as follows Figure 3 As shown, Figure 3 The figure is a block diagram of the adaptive PI filter bandwidth adjustment structure, which describes the whole process of adaptively adjusting the filter bandwidth Bn using the difference between the speed variance and the original speed.

[0068] (1) First, the bandwidth adjustment module adjusts the acceleration and deceleration state bandwidth component B according to the current motion state sent by the acceleration and deceleration judgment module. n1 Adjust. Acceleration / deceleration state bandwidth component B n1 Associated with acceleration and deceleration, the acceleration and deceleration state bandwidth component B during the acceleration process n1 Small, the acceleration and deceleration state bandwidth component B during the deceleration process n1 big.

[0069] If it is in the acceleration state, the acceleration / deceleration state bandwidth component B n1 Equal to the maximum value of the preset acceleration / deceleration state bandwidth component; if it is in the deceleration state, the acceleration / deceleration state bandwidth component B n1 Equal to the minimum value of the preset acceleration and deceleration state bandwidth component; if it is a uniform speed state, the acceleration and deceleration state bandwidth component B n1 Equal to the median value of the preset acceleration / deceleration state bandwidth component.

[0070] (2) Then, the bandwidth adjustment module adjusts the speed variance bandwidth component B according to the speed variance sent by the data acquisition module. n0 Adjust the speed variance bandwidth component B n0 Associated with the speed variance, the larger the speed variance, the larger the speed variance bandwidth component B n0 The smaller.

[0071] In this embodiment, the velocity variance bandwidth component B n0 It is equal to the product of the maximum value of the preset speed variance bandwidth component and the preset scaling factor k0.

[0072] (3) Finally, the bandwidth adjustment module adjusts the bandwidth component B according to the acceleration and deceleration state. n1 and velocity variance bandwidth component B n0 , obtain the adjusted adaptive PI filter bandwidth Bn, and send the adjusted bandwidth Bn to the adaptive PI filtering module.

[0073] The adjusted bandwidth Bn is equal to the acceleration / deceleration state bandwidth component B n1 and velocity variance bandwidth component B n0 The product of .

[0074] The adaptive PI filtering module uses the adjusted bandwidth Bn sent by the bandwidth adjustment module to perform adaptive PI smoothing filtering on the original speed sent by the data acquisition module, thereby obtaining the filtered speed at the current moment, and sends the filtered speed at the current moment to the result output module.

[0075] The specific calculation method is: First, calculate the difference between the original speed at the current moment sent by the data acquisition module and the filtered speed at the previous moment output by the result output module to obtain the error e(t); then, based on the integral value of the error from time 0 to the previous sampling moment, and the product of the I coefficient at the current moment and the error e(t), the updated integral term at the current moment (that is, the integral value of the error from time 0 to the current sampling moment) is obtained. Finally, based on the filtered speed at the previous moment, the updated integral value, and the product of the P coefficient and the error, the filtered speed at the current moment is obtained.

[0076] The result output module outputs the filtered speed at the current moment as the final output for subsequent navigation applications.

[0077] This embodiment provides a speed smoothing filter system based on adaptive PI filtering. By combining the original speed and speed variance, an adaptive PI filter is introduced to adaptively adjust the filter bandwidth B according to the speed acceleration and deceleration and speed variance. n , achieving effective smoothing filtering of the PVT solution speed. Adopting an asymmetric filtering strategy, it strengthens filtering during acceleration and reduces filtering during deceleration, improving navigation accuracy and reliability.

[0078] Example 3: This embodiment provides a speed smoothing filtering method based on adaptive PI filtering, and specific data is introduced to implement the speed smoothing filtering method based on adaptive PI filtering of embodiment 1.

[0079] The data in this embodiment is described using speed data at a single moment, specifically as follows: 1. Initialization.

[0080] When the system starts, the adaptive PI filter parameters are initialized, including: the speed variance bandwidth component B n0 The maximum value of is set to 1.0, the scaling factor k0 is set to 1.0, and the acceleration / deceleration state bandwidth component B is set to 1.0. n1 The minimum value of the acceleration / deceleration state bandwidth component B is set to 0.3. n1 The median value of is set to 0.5, and the acceleration / deceleration state bandwidth component B n1 The maximum value of is set to 0.7 and the fixed coefficient k is set to 2.0.

[0081] At the same time, initialize the integral term (that is, the error e(i) at time 0 is equal to 0) and the filtered speed at the previous moment, that is, the filtered speed v at the previous moment filtered (t-1) is equal to 0.

[0082] 2. Data acquisition.

[0083] At the current sampling moment, the original velocity v is read from the PVT solution module.raw (t) is 19.4m / s, and the reading speed variance σ 2 (t) is 0.12.

[0084] The error integral value accumulated from time 0 to the last sampling time is -0.1412. (That is, the product of the square of the adjusted bandwidth and the error accumulated from time 0 to the last sampling time) 3. Acceleration and deceleration judgment.

[0085] Calculate the difference between the current speed and the previous speed (the value is 20.23m / s), and get the difference dv as -0.83m / s. Based on the positive or negative judgment of the difference dv, it can be concluded that the current state is deceleration.

[0086] 4. Adaptive PI filter bandwidth adjustment.

[0087] Calculate the velocity variance bandwidth component B based on the velocity variance n0 , the velocity variance bandwidth component is equal to the velocity variance bandwidth component B n0 The product of the maximum value of and the scaling factor k0.

[0088] Since the velocity variance is less than 0.2, the scaling factor k0 can reach a maximum value of 1.0. n0 The maximum value of is 1.0, so the velocity variance bandwidth component B n0 is 1.0.

[0089] Determine the acceleration / deceleration state bandwidth component B according to the acceleration / deceleration state n1 Value: According to the above judgment, the current state is deceleration, and the acceleration and deceleration state bandwidth component B during deceleration n1 Equal to the acceleration / deceleration state bandwidth component B n1 The maximum value of is 0.7).

[0090] Calculate the adjusted adaptive PI filter bandwidth B n : Adaptive PI filter bandwidth B n Equal to the velocity variance bandwidth component B n0 The bandwidth component B of the acceleration and deceleration state during deceleration n1 The product of is 0.7.

[0091] 5. Adaptive PI filtering: Calculate the P coefficient of the PI filter: The P coefficient is equal to the fixed coefficient and the adjusted adaptive PI filter bandwidth B n The product of is 1.4.

[0092] Calculate the P coefficient of the PI filter: The I coefficient is equal to the adjusted adaptive PI filter bandwidth B nThe square of , which is 0.49.

[0093] Calculate the error e(t) at the current moment: The error at the current moment is equal to the difference between the original velocity at the current moment and the filtered velocity at the previous moment.

[0094] In this embodiment, the filtered speed at the last moment is 19.92 m / s, so the error e(t) is equal to -0.52 m / s.

[0095] Update the integral term: The error integral value accumulated from time 0 to the current sampling time (that is, the product of the square of the adjusted bandwidth (that is, the I coefficient) and the error accumulated from time 0 to the current sampling time) is equal to the error integral value accumulated from time 0 to the previous sampling time (-0.1412 obtained in step 2) and the product of the square of the adjusted bandwidth and the error at the current sampling time. The updated integral term is -0.396.

[0096] Calculate the filtered speed: The filtered speed at the current moment is equal to the filtered speed at the previous moment, plus the product of the P coefficient and the error, plus the sum of the updated integral values. The filtered speed at the current moment is 18.796 m / s.

[0097] 6. Output the results.

[0098] The filtered velocity at the current moment is output to the subsequent navigation application module. The new integral value -0.396 is stored and prepared for use at the next sampling moment.

[0099] Comparing the results with the baseline value: The current baseline speed is 18.93 m / s, the original speed is 19.4 m / s, and the difference from the baseline is 0.47 m / s. The filtered speed is 18.796 m / s, and the difference from the baseline is 0.134 m / s. It can be seen that the filtered speed is closer to the baseline.

[0100] Therefore, by using the speed smoothing filtering method based on adaptive PI filtering provided in the first embodiment, a more accurate speed value can be obtained, thereby improving navigation accuracy and reliability.

[0101] The embodiment described above is only a preferred solution of the present invention and does not limit the present invention in any form. Other variations and modifications are possible without exceeding the technical solution described in the claims.

Claims

1. A velocity smoothing filtering method based on adaptive PI filtering, characterized in that: include: S1: Get the original speed and speed variance at the current moment obtained by PVT solution; S2: Calculate the speed difference between the current moment and the previous moment, and determine the current motion state based on the speed difference; S3: Adaptively adjust the bandwidth of the adaptive PI filter according to the velocity variance and the current motion state; S4: Calculate the P coefficient and I coefficient of the adaptive PI filter using the adjusted bandwidth, and perform smoothing filtering on the original speed to obtain the filtered speed.

2. The velocity smoothing filtering method based on adaptive PI filtering according to claim 1, characterized in that: The smoothing filtering of the original speed to obtain the filtered speed includes: Calculate the P coefficient and I coefficient of the PI filter; calculate the error between the current original speed and the filtered speed at the previous moment; use the I coefficient and speed error to calculate the integral component of the speed filter at the current moment and update the integral term; use the filtered speed and P coefficient at the previous moment, as well as the updated integral term, to calculate the filtered speed at the current moment.

3. The velocity smoothing filtering method based on adaptive PI filtering according to claim 1, characterized in that: The S3 includes: the bandwidth of the adaptive PI filter is the product of the speed variance bandwidth component and the acceleration / deceleration state bandwidth component, the speed variance bandwidth component is adjusted according to the speed variance: the larger the speed variance, the smaller the speed variance bandwidth component; the acceleration / deceleration state bandwidth component is adjusted according to the current motion state: the acceleration / deceleration state bandwidth component is small during the acceleration process, and the acceleration / deceleration state bandwidth component is large during the deceleration process.

4. The velocity smoothing filtering method based on adaptive PI filtering according to claim 3, characterized in that: Adjusting the acceleration / deceleration state bandwidth component according to the current motion state includes: if it is an acceleration state, the acceleration / deceleration state bandwidth component is a minimum value of the preset acceleration / deceleration state bandwidth component; if it is a deceleration state, the acceleration / deceleration state bandwidth component is a maximum value of the preset acceleration / deceleration state bandwidth component; if it is a uniform speed state, the acceleration / deceleration state bandwidth component is a median value of the preset acceleration / deceleration state bandwidth component.

5. The velocity smoothing filtering method based on adaptive PI filtering according to claim 2, characterized in that: The P coefficient is k times the bandwidth of the adaptive PI filter after the update, k is a fixed coefficient, and the I coefficient is the square of the bandwidth of the adaptive PI filter after the update.

6. A velocity smoothing filtering method based on adaptive PI filtering according to claim 3 or 4, characterized in that: The velocity variance bandwidth component is the product of the maximum value of the velocity variance bandwidth component and a scaling factor, and the scaling factor is a monotonically decreasing function of the velocity variance.

7. A velocity smoothing filtering method based on adaptive PI filtering according to claim 1, 2, 3, 4 or 5, characterized in that: The S1 further includes: initializing the relevant parameters of the adaptive PI filter, and simultaneously initializing the integral term and the speed after filtering at the previous moment.

8. A velocity smoothing filtering method based on adaptive PI filtering according to claim 2 or 5, characterized in that: Obtain the filtered speed at the previous moment, obtain the product of the P coefficient and the error at the current moment, obtain the sum of the products of the I coefficient and the error at each moment from 0 to the current moment, and the filtered speed is the sum of the three.

9. A velocity smoothing filtering method based on adaptive PI filtering according to claim 1, 2, 3, 4 or 5, characterized in that: In the above S2, if the speed difference is greater than 0, it is an acceleration state; if the speed difference is less than 0, it is a deceleration state; if the speed difference is 0, it is a constant speed state.

10. A velocity smoothing filter system based on adaptive PI filtering, characterized in that: include: The data acquisition module obtains the original speed and speed variance at the current moment obtained by PVT solution; The acceleration and deceleration judgment module calculates the speed difference between the current moment and the previous moment, and determines the current motion state based on the speed difference; The bandwidth adjustment module adaptively adjusts the bandwidth of the adaptive PI filter according to the velocity variance and the current motion state; The adaptive PI filter module calculates the P coefficient and I coefficient of the adaptive PI filter using the adjusted bandwidth, and performs smoothing filtering on the original speed to obtain the filtered speed.

Images