High dynamic environment pose estimation method and system based on IMU data screening

Abstract

The invention provides a high dynamic environment pose estimation method and system based on IMU data screening, and relates to the field of automatic driving, and the method comprises the steps: detecting initial IMU data of a bumpy road section, setting the size of a sliding window, screening the initial IMU data based on the sliding window, and obtaining corresponding slope data; screening out data of which the widths of wave crests and wave troughs are greater than a threshold value from the slope data as screened data; performing anomaly detection on the screened data, and performing smoothing processing on abnormal data to obtain optimized IMU data; and inputting the initial IMU data and the optimized IMU data into a radar inertial odometer, and calculating to obtain a pose estimation result. According to the invention, through real-time robust IMU data detection, the problem that the radar inertia fails when the unmanned vehicle passes through the deceleration strip and the bumpy road data is solved, and the map with high local consistency is obtained, so that the influence of local bumpy road sections on the odometer is eliminated.

Description

Technical Field

[0001] This invention relates to the field of autonomous driving technology, and specifically to a pose estimation method and system for high-dynamic environments based on IMU data filtering. Background Technology

[0002] As the mass production scale of autonomous driving continues to expand, Level 4 autonomous driving is becoming increasingly reliant on maps. Accurate and robust 3D reconstruction algorithms play a crucial role in key stages such as lightweight map production and perception model annotation. Currently, Simultaneous Localization and Mapping (SLAM) systems based on laser point clouds have become the main technical method in the field of 3D reconstruction due to their high accuracy and minimal susceptibility to lighting conditions.

[0003] However, in practical applications, it has been found that current mainstream radar inertial algorithms exhibit significant shortcomings in highly dynamic environments. In high-dynamic scenarios such as speed bumps and uneven roads, the algorithms perform poorly, frequently resulting in mapping failures and severely hindering production efficiency. This is because, in highly dynamic environments, inertial measurement unit (IMU) data causes drastic changes in the position and attitude of the odometry, significantly affecting the smoothness of the trajectory. Summary of the Invention

[0004] This invention addresses the problems existing in the prior art by providing a pose estimation method and system for high-dynamic environments based on IMU data filtering. The IMU data filtering mechanism is based on sliding window and slope method, thereby enabling radar inertial algorithms to obtain stable pose estimates in high-dynamic environments.

[0005] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0006] Detect initial IMU data for bumpy road sections, set the sliding window size, filter the initial IMU data based on the sliding window, and obtain the corresponding slope data; In the slope data, select data whose peak and trough widths are greater than a threshold and use them as filter data; Anomaly detection is performed on the filtered data, and the abnormal data is smoothed to obtain optimized IMU data; After performing secondary saturation filtering on the initial IMU data and optimized IMU data, the data is input into the radar inertial odometry to calculate the pose estimation result.

[0007] In some embodiments, the process of setting the sliding window size and filtering the initial IMU data based on the sliding window to obtain the corresponding slope data includes: Set the slider window size parameter; The initial IMU data enters the tail of the sliding window and begins to slide sequentially, obtaining several local IMU data points; The slope data is calculated by fitting the straight lines corresponding to the local IMU data using the least squares method.

[0008] In some embodiments, the process of filtering out data in the slope data whose peak and trough widths are greater than a threshold includes: Plot the slope data based on its positive and negative values, identify peaks and troughs. A trough appears when the slope changes from positive to negative, and a peak appears when it changes from positive to negative. Calculate the index position information of all peaks and troughs. The starting positions of the peaks and troughs are determined based on the index position information, and the widths of the corresponding peaks and troughs are calculated based on the starting positions. The widths of the peaks and troughs are compared with preset thresholds, and peaks and troughs with widths greater than the thresholds are selected as filter data. The time length within the range of peaks and troughs is used as the width.

[0009] In some embodiments, the process of performing anomaly detection on the filtered data, smoothing the abnormal data, and obtaining optimized IMU data includes: Calculate the quantiles of the initial IMU data range; Anomaly detection is performed based on the extreme values ​​of peaks and troughs, and results that exceed the range of the initial IMU data are selected as outlier data. For outlier data, replace it with the value of the adjacent normal data point to obtain smoothed data; By integrating smoothed data with data that has passed anomaly detection, optimized IMU data is obtained.

[0010] In some embodiments, the process of inputting initial IMU data and optimized IMU data into a radar inertial odometer after secondary saturation filtering to calculate the pose estimation result includes: Saturation is calculated for the initial IMU data and the optimized IMU data respectively to obtain saturation index. The saturation index exceeding the threshold is used as the initial saturated IMU data and the optimized saturated IMU data. The initial saturated IMU data and the optimized saturated IMU data are input into the radar inertial odometer; The pose of an object is estimated using Kalman filtering to obtain the pose estimation result.

[0011] In some embodiments, the process of detecting initial IMU data for bumpy road sections includes: Error data from IMU data on bumpy road sections is detected, and after deleting the error data, the data is arranged in chronological order to obtain preprocessed data; The pitch angular velocity data is extracted from the preprocessed data and used as the initial IMU data.

[0012] In some embodiments, the process of comparing the width of each peak and trough with a preset threshold and filtering out peaks and troughs with widths greater than the threshold as data filtering includes: If the width of the crest and trough is 0, the starting position is reset to the current time; If the width of the peak and trough is less than the threshold, it is ignored and the starting position remains unchanged; Continue testing the slope data until the filter width is greater than the threshold, at which point the test ends.

[0013] This invention proposes a pose estimation system for high-dynamic environments based on IMU data filtering, comprising: A sliding window unit is configured to detect initial IMU data on bumpy road sections. The sliding window size is set, and the initial IMU data is filtered based on the sliding window to obtain the corresponding slope data. The filtering unit is configured to filter out data in the slope data whose peak and trough widths are greater than a threshold, and use this data as the filtered data. The smoothing unit is configured to perform anomaly detection on the filtered data and smooth the abnormal data to obtain optimized IMU data. The secondary filtering unit is configured to perform secondary saturation filtering on the initial IMU data and the optimized IMU data before inputting them into the radar inertial odometry to calculate the pose estimation result.

[0014] This invention proposes an unmanned vehicle, comprising: At least one processor; and a memory storing a computer program executable on the processor, wherein the processor, when executing the program, performs the steps of the pose estimation method for a high-dynamic environment based on IMU data filtering.

[0015] This invention proposes a computer-readable storage medium / a computer program product. The computer-readable storage medium stores computer-executable instructions, which, when executed by a processor, perform the pose estimation method for high-dynamic environments based on IMU data filtering; and / or, The computer program includes a computer program that, when executed by a processor, implements the pose estimation method for high dynamic environments based on IMU data filtering.

[0016] Compared with the prior art, the present invention has the following beneficial effects: This invention proposes a pose estimation method and system for high-dynamic environments based on IMU data filtering. The method includes: detecting initial IMU data of a bumpy road section, setting a sliding window size, filtering the initial IMU data based on the sliding window to obtain corresponding slope data; filtering the slope data to select data whose peak and trough widths are greater than a threshold as filtered data; performing anomaly detection on the filtered data, smoothing the anomaly data to obtain optimized IMU data; and inputting the initial IMU data and optimized IMU data into a radar inertial odometry system to calculate the pose estimation result.

[0017] This invention solves the problem of radar inertial detection failing to detect data when unmanned vehicles pass over speed bumps and bumpy roads by using real-time robust IMU data detection, thus obtaining a map with high local consistency.

[0018] The real-time robust IMU data detection method is based on the slope method of sliding window to filter IMU bumps. The window size is fixed, and IMU data enters from the tail of the window. Peaks and troughs are filtered out according to the positive and negative changes of the slope, and results with smaller widths are filtered out according to the width threshold.

[0019] For data exceeding the IMU's measurement range, due to localized data consistency, adjacent data are used to replace abnormal peaks and troughs. The filtered results undergo a second saturation filtering process before being input into FastLIO-2 as the prediction result. This eliminates the impact of localized bumpy road sections on the odometer. Attached Figure Description

[0020] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other embodiments can be obtained based on these drawings without creative effort.

[0021] Figure 1 The flowchart shows the pose estimation method for high dynamic environments based on IMU data filtering provided by this invention.

[0022] Figure 2 This is a block diagram of a pose estimation system for high-dynamic environments based on IMU data filtering, provided by the present invention.

[0023] Figure 3 This is a schematic diagram of the structure of an embodiment of the unmanned vehicle provided by the present invention.

[0024] Figure 4 This is a schematic diagram of an embodiment of the computer-readable storage medium provided by the present invention.

[0025] Figure 5This is a flowchart of FastLIO2, an embodiment of the pose estimation method for high dynamic environments based on IMU data filtering provided by the present invention.

[0026] Figure 6 This is an abnormal peak and valley detection map of an embodiment of the pose estimation method for high dynamic environments based on IMU data filtering provided by the present invention.

[0027] Figure 7 This is an optimized point cloud ground data map of an embodiment of the pose estimation method for high dynamic environments based on IMU data filtering provided by the present invention.

[0028] Figure 8 This is an optimized point cloud ground data map of an embodiment of the pose estimation method for high dynamic environments based on IMU data filtering provided by the present invention. Detailed Implementation

[0029] The present invention will be further described below with reference to the accompanying drawings. The following embodiments are only used to more clearly illustrate the technical solutions of the present invention and should not be construed as limiting the scope of protection of the present invention. It should be noted that the following detailed descriptions are exemplary and intended to provide further explanation of this application.

[0030] It should be noted that all uses of "first" and "second" in the embodiments of the present invention are for the purpose of distinguishing two entities or parameters with the same name but different names. It is clear that "first" and "second" are only for the convenience of expression and should not be construed as limiting the embodiments of the present invention. Subsequent embodiments will not explain this in detail.

[0031] This invention proposes a pose estimation method for high-dynamic environments based on IMU data filtering. Please refer to [link / reference]. Figure 1 , Figure 7 and Figure 8 ,include: S1. Detect the initial IMU data of the bumpy road section, set the sliding window size, filter the initial IMU data based on the sliding window, and obtain the corresponding slope data; S2. Filter out data in the slope data where the width of the peaks and troughs is greater than the threshold, and use this data as the filter data; S3. Perform anomaly detection on the filtered data and smooth the abnormal data to obtain optimized IMU data; S4. After performing secondary saturation filtering on the initial IMU data and optimized IMU data, input them into the radar inertial odometry to calculate the pose estimation result.

[0032] This invention accurately detects and acquires raw IMU data for bumpy road sections. A sliding window size is set, and the window filters the data to obtain corresponding slope data, which reflects the changing trend of the IMU data. Data with peak and trough widths exceeding a threshold are selected from the slope data for filtering. The focus is on IMU data changes with significant characteristics, eliminating interference from minor fluctuations. Anomaly detection is performed on the filtered data to avoid affecting the accuracy of calculations; anomalies are smoothed to obtain optimized IMU data, making the data more stable and reliable.

[0033] The initial IMU data and optimized IMU data are input into the radar inertial odometry (IOME) after undergoing a second saturation filtering process. This second saturation filtering ensures the quality of the input data. The IOME uses this processed, high-quality data for calculations, resulting in more accurate pose estimation. The accuracy of pose estimation has been improved from a relatively large error to a high level, effectively increasing the success rate of mapping.

[0034] This invention primarily optimizes IMU input data, detecting and smoothing IMU data on bumpy road sections. This results in smoother forward propagation data and avoids point cloud layering.

[0035] as follows Figure 7 As shown, before optimization, the point cloud ground plane breaks at locations with abnormal IMU data. After optimization, it is as follows: Figure 8 As shown, the fracture points become smoother. This makes the generated map more complete and accurate, reflecting the actual road environment more realistically. It reduces mapping failures or errors caused by data anomalies and improves robustness in complex dynamic environments.

[0036] In some embodiments, please refer to Figure 1 The process of setting the sliding window size and filtering the initial IMU data based on the sliding window to obtain the corresponding slope data includes: Set the slider window size parameter; The initial IMU data enters the tail of the sliding window and begins to slide sequentially, obtaining several local IMU data points; The slope data is calculated by fitting the straight lines corresponding to the local IMU data using the least squares method.

[0037] In this invention, the initial IMU data is sequentially slid from the end of a sliding window. As the window slides, several local IMU data points are obtained. This allows the characteristics of the IMU data over a short period of time to be reflected from different locations.

[0038] The least squares method finds the best function match for the data by minimizing the sum of squared errors. After fitting a straight line, the slope data is calculated. The slope data can intuitively reflect the rate of change and trend of IMU data within a corresponding local range.

[0039] For example, in data acquisition on a bumpy road section during autonomous driving, a sliding window size of 15 data points is set. Initial IMU data are sequentially entered into the sliding window, and each slide yields a local IMU data block. Assuming a local IMU data block contains 15 data points, after fitting a straight line using the least squares method, the calculated slope is 0.8. This slope reflects the variation of IMU data within that local area. By processing all local IMU data in this way, a series of slope data can be obtained.

[0040] In some embodiments, please refer to Figure 1 and Figure 6 The process of filtering out data from the slope data whose peak and trough widths are greater than a threshold, as the filtered data, includes: Plot the slope data based on its positive and negative values, identify peaks and troughs. A trough appears when the slope changes from positive to negative, and a peak appears when it changes from positive to negative. Calculate the index position information of all peaks and troughs. The starting positions of the peaks and troughs are determined based on the index position information, and the widths of the corresponding peaks and troughs are calculated based on the starting positions. The width of each peak and trough is compared with a preset threshold, and peaks and troughs with widths greater than the threshold are selected as the filtered data. The time length within the range of peaks and troughs is used as the width.

[0041] This invention transforms slope data into intuitive graphs. Peaks and troughs are identified by observing changes in the slope. Specifically, a change in slope from positive to negative indicates a trough, and vice versa. After identifying peaks and troughs, the index positions of all peaks and troughs are calculated. These index positions are data coordinates, allowing for precise location of peaks and troughs within the data sequence.

[0042] The time length within the peak and trough range is defined as the width. The width of each peak and trough is compared with a pre-set threshold. Only peaks and troughs with a width greater than the threshold are filtered out. Peaks and troughs with significant characteristics and sufficient time span are used as filtering data to exclude interference from some short-term and minor fluctuations.

[0043] For example, in processing a segment of IMU slope data, the preset threshold for peak and trough widths is 5 time units. Multiple peaks and troughs are identified through plotting, and their index positions are statistically determined. Calculations show that one peak has a width of 7 time units, and another trough has a width of 6 time units. Since their widths both exceed the preset threshold of 5 time units, these two peaks and troughs are selected as filtered data.

[0044] Figure 6 As shown, when the autonomous vehicle traverses an abnormally bumpy road surface, it experiences both overweight and weightlessness, resulting in peaks and troughs in the pitch angular velocity of the IMU data. This algorithm utilizes a sliding window-based filtering method. First, the sliding window size is fixed, allowing the IMU's pitch angular velocity data to pass through the end of the window, and a derivative, or slope, is calculated. A trough occurs when the slope changes from positive to negative, and vice versa. The width of the peak and trough is recorded. If the value is 0, the starting position is reset to the current time. If the width is less than a threshold, it is ignored, the starting position remains unchanged, and IMU data continues to be detected. This process continues until a result with a width greater than the threshold is selected as the termination position.

[0045] In some embodiments, please refer to Figure 1 The process of performing anomaly detection on the filtered data, smoothing the abnormal data, and obtaining optimized IMU data includes: Calculate the quantiles of the initial IMU data range; Anomaly detection is performed based on the extreme values ​​of peaks and troughs, and results that exceed the range of the initial IMU data are selected as outlier data. For outlier data, replace it with the value of the adjacent normal data point to obtain smoothed data; By integrating smoothed data with data that has passed anomaly detection, optimized IMU data is obtained.

[0046] Quantiles are key numerical points in statistics used to describe the distribution of data. They divide a set of ordered data into several equal parts, reflecting the central tendency of the data in different intervals. Specifically, in this scheme, a quantile of 95% means that 95% of the values ​​in the data are less than or equal to this value; the remaining 5% of the values ​​are greater than this value, close to the upper limit of the range.

[0047] Based on the extreme values ​​of peaks and troughs, results exceeding the 95th percentile of the IMU measurement range were filtered out, as data close to the range were considered insufficiently accurate. For detected abnormal peaks and troughs, since IMU data exhibits consistency within a local range, adjacent data were used to fill in the missing data.

[0048] This invention detects anomalies based on extreme values ​​at peaks and troughs. The extreme values ​​are compared to the previously calculated 95th percentile. If an extreme value at a peak or trough exceeds this 95th percentile, the corresponding data point is considered an anomaly. For detected anomalies, a smoothing process is performed by replacing the values ​​with those of adjacent normal data points. This eliminates the abrupt changes caused by anomalies, making the data sequence more coherent and stable.

[0049] Finally, the smoothed data is integrated with the normal data that passed anomaly detection. The integrated data removes anomaly interference while retaining the effective information of the original data, thus obtaining optimized IMU data.

[0050] For example, in a certain run, the 95th percentile of the initial IMU data range was calculated to be 12. In anomaly detection based on peak and trough extremes, a peak extreme of 15 was found, exceeding 12. The data point corresponding to this peak was determined to be an anomaly. The anomaly data point was replaced with the average of its adjacent normal data points (e.g., two adjacent normal data points with values ​​of 10 and 11, respectively), which is 10.5. The replaced smoothed data was then integrated with other normal data to obtain the optimized IMU data.

[0051] In some embodiments, please refer to Figure 1 and Figure 5 The process of inputting the initial IMU data and optimized IMU data into the radar inertial odometry after secondary saturation filtering to calculate the pose estimation results includes: Saturation is calculated for the initial IMU data and the optimized IMU data respectively to obtain saturation index. The saturation index exceeding the threshold is used as the initial saturated IMU data and the optimized saturated IMU data. The initial saturated IMU data and the optimized saturated IMU data are input into the radar inertial odometer; The pose of an object is estimated using Kalman filtering to obtain the pose estimation result.

[0052] This invention inputs the IMU data before and after optimization into a radar inertial odometry system, respectively. The smoothed IMU data yields accurate results. The radar inertial odometry system estimates the object's pose using Kalman filtering, and the process for obtaining the pose estimation result is as follows. Figure 5 As shown in the flowchart, its core is the state estimation module, which forward procedurally calculates the predicted pose from the input IMU data. Simultaneously, it uses the predicted pose to perform backpropagation to correct distortion in the input point cloud. Finally, it constructs point-surface residuals to update the pose state.

[0053] This invention calculates the saturation of initial IMU data and optimized IMU data separately, obtaining corresponding saturation indices. The saturation index measures the richness of data within a certain range, reflecting whether the data is within a reasonable operating range. The calculated saturation index is then compared with a preset threshold, and data exceeding the threshold are identified as initial saturated IMU data and optimized saturated IMU data, respectively. This effectively filters out data that affects calculation accuracy, ensuring high reliability of the data input to the odometer.

[0054] Radar inertial odometry, as an advanced algorithmic tool, can efficiently process input data. During processing, the Kalman filter method is used to estimate the object's pose. Kalman filtering is a classic state estimation method that continuously optimizes the object's pose estimation through two stages: prediction and update. This effectively reduces noise interference and improves the accuracy and stability of the estimation.

[0055] For example, in a certain data processing scenario, the initial IMU data saturation index is calculated to be 0.8, the optimized IMU data saturation index is 0.75, and the preset saturation threshold is 0.7. The portion of the initial IMU data with a saturation index exceeding 0.7 is then considered the initial saturated IMU data. Similarly, optimized saturated IMU data is selected from the optimized IMU data. These data are then input into the radar inertial odometry system, and after Kalman filtering, accurate pose estimation results are obtained.

[0056] In some embodiments, please refer to Figure 1 The process of detecting the initial IMU data for bumpy road sections includes: Error data from IMU data on bumpy road sections is detected, and after deleting the error data, the data is arranged in chronological order to obtain preprocessed data; The pitch angular velocity data is extracted from the preprocessed data and used as the initial IMU data.

[0057] This invention comprehensively detects IMU data from bumpy road sections, deletes erroneous data, and arranges the remaining data in chronological order to obtain preprocessed data.

[0058] The preprocessed data contains information from multiple directions and types. Data closely related to the characteristics of bumpy road sections is selected, and pitch angular velocity data is extracted from the preprocessed data. Pitch angular velocity can intuitively reflect the severity of the vehicle's vertical vibration on bumpy road sections. It is used as the initial IMU data to analyze the impact of bumpy road sections on mapping and data optimization.

[0059] In a scenario where an autonomous vehicle traverses a bumpy road, the detected IMU data contains a large number of data points, some of which are erroneous and deviate significantly from the normal range, such as abnormally large or small angular velocity values. After deleting these erroneous data, the data is sorted chronologically to obtain preprocessed data. Further, pitch angular velocity data is extracted from the preprocessed data. Assuming that the extracted pitch angular velocity at a certain moment is 0.5 radians / second, this data is used as the initial IMU data for subsequent processing.

[0060] In some embodiments, the process of comparing the width of each peak and trough with a preset threshold and filtering out peaks and troughs with widths greater than the threshold as data filtering includes: If the width of the crest and trough is 0, the starting position is reset to the current time; If the width of the peak and trough is less than the threshold, it is ignored and the starting position remains unchanged; Continue testing the slope data until the filter width is greater than the threshold, at which point the test ends.

[0061] When this invention encounters a situation where the width of a peak or trough is 0, it means that at the current data analysis node, the characteristic manifestation of the peak or trough is extremely brief, almost negligible in terms of actual time span. In this case, the starting position is reset to the current time, and the starting point for subsequent analysis is redefined.

[0062] If the width of the peaks and troughs is less than the preset threshold, it indicates that the duration of these peaks and troughs is short, and they may only be some minor fluctuations in the data, not reflecting significant bumpy road sections. Therefore, we choose to ignore them and keep the starting position unchanged to continue analyzing the subsequent slope data, avoiding interference from these minor fluctuations with the overall screening results.

[0063] Only when the width of the peak and trough is detected to be greater than the preset threshold is it used as the filtering data, and this is used as the termination position to complete the filtering work of this stage.

[0064] For example, the preset threshold for peak and trough width is 5 time units. During the analysis, a peak with a width of 0 appears, at which point the starting position is reset to the current time; then a trough with a width of 2 time units is found, which is ignored since it is less than the threshold and the starting position remains unchanged; subsequently, a peak with a width of 7 time units is detected, which is used as the filtered data because it is greater than the threshold, and the position of this peak is used as the ending position.

[0065] This invention proposes a pose estimation system for high-dynamic environments based on IMU data filtering. Please refer to [link / reference]. Figure 2 ,include: The sliding window unit 100 is configured to detect the initial IMU data of bumpy road sections. The sliding window size is set, and the initial IMU data is filtered based on the sliding window to obtain the corresponding slope data. The filtering unit 200 is configured to filter out data in the slope data whose peak and trough widths are greater than a threshold, and use them as the filtered data. Smoothing unit 300 is configured to perform anomaly detection on the filtered data and smooth the abnormal data to obtain optimized IMU data; The secondary filtering unit 400 is configured to perform secondary saturation filtering on the initial IMU data and the optimized IMU data and then input them into the radar inertial odometry to calculate the pose estimation result.

[0066] Based on the same inventive concept, according to another aspect of the present invention, such as Figure 3 As shown, an embodiment of the present invention also provides an unmanned vehicle 30, which includes a processor 310 and a memory 320. The memory 320 stores a computer program 321 that can run on the processor. When the processor 310 executes the program, it performs the steps of the method described above.

[0067] Based on the same inventive concept, according to another aspect of the present invention, such as Figure 4 As shown, embodiments of the present invention also provide a computer-readable storage medium 40, which stores a computer program 410 that, when executed by a processor, performs the methods described above.

[0068] The present invention also proposes a computer program product, which includes a computer program that, when executed by a processor, implements the pose estimation method for high dynamic environments based on IMU data filtering.

[0069] Embodiments of the present invention may also include a corresponding unmanned vehicle. The unmanned vehicle includes a memory, at least one processor, and a computer program stored in the memory and executable on the processor, wherein the processor executes any of the methods described above when executing the program.

[0070] The memory, as a non-volatile computer-readable storage medium, can be used to store non-volatile software programs, non-volatile computer-executable programs, and modules, such as program instructions / modules in the embodiments of this application. The processor executes various functional applications and data processing of the device by running the non-volatile software programs, instructions, and modules stored in the memory, thereby implementing the above-described method.

[0071] The memory may include a program storage area and a data storage area. The program storage area may store the operating system and applications required for at least one function; the data storage area may store data created based on the use of the device. Furthermore, the memory may include high-speed random access memory and non-volatile memory, such as at least one disk storage device, flash memory device, or other non-volatile solid-state storage device. In embodiments, the memory may optionally include memory remotely located relative to the processor, which can be connected to the local module via a network. Examples of such networks include, but are not limited to, the Internet, corporate intranets, local area networks, mobile communication networks, and combinations thereof.

[0072] Finally, it should be noted that those skilled in the art will understand that all or part of the processes in the above embodiments can be implemented by a computer program instructing related hardware. The program can be stored in a computer-readable storage medium, and when executed, it can include the processes of the embodiments of the methods described above. The storage medium for the program can be a magnetic disk, optical disk, read-only memory (ROM), or random access memory (RAM), etc. The above computer program embodiments can achieve the same or similar effects as any of the corresponding foregoing method embodiments.

[0073] Those skilled in the art will also understand that the various exemplary logic blocks, modules, circuits, and algorithm steps described in conjunction with the disclosure herein can be implemented as electronic hardware, computer software, or a combination of both. To clearly illustrate this interchangeability between hardware and software, the functionality of various illustrative components, blocks, modules, circuits, and steps has been generally described. Whether this functionality is implemented as software or as hardware depends on the specific application and the design constraints imposed on the system as a whole. Those skilled in the art can implement the functionality in various ways for each specific application, but such implementation decisions should not be construed as departing from the scope of the embodiments disclosed herein.

[0074] The above are exemplary embodiments disclosed in this invention. However, it should be noted that various changes and modifications can be made without departing from the scope of the embodiments of this invention as defined by the claims. The functions, steps, and / or actions of the methods according to the disclosed embodiments described herein do not need to be performed in any particular order. The sequence numbers of the disclosed embodiments of this invention are for descriptive purposes only and do not represent the superiority or inferiority of the embodiments. Furthermore, although the elements disclosed in the embodiments of this invention may be described or claimed individually, they may be understood as multiple unless explicitly limited to a singular number.

[0075] It should be understood that, as used herein, the singular form “a” is intended to include the plural form as well, unless the context clearly supports an exception. It should also be understood that, as used herein, “and / or” refers to any and all possible combinations of one or more of the associated listed items.

[0076] Those skilled in the art should understand that the discussion of any of the above embodiments is merely exemplary and is not intended to imply that the scope of the invention (including the claims) is limited to these examples. Within the framework of the invention, technical features of the above embodiments or different embodiments can be combined, and many other variations of different aspects of the invention exist, which are not provided in the details for the sake of brevity. Therefore, any omissions, modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the invention should be included within the protection scope of the invention.

Claims

1. A pose estimation method for high-dynamic environments based on IMU data filtering, characterized in that, include: Detect initial IMU data for bumpy road sections, set the sliding window size, filter the initial IMU data based on the sliding window, and obtain the corresponding slope data; In the slope data, select data whose peak and trough widths are greater than a threshold and use them as filter data; Anomaly detection is performed on the filtered data, and the abnormal data is smoothed to obtain optimized IMU data; After performing secondary saturation filtering on the initial IMU data and optimized IMU data, the data is input into the radar inertial odometry to calculate the pose estimation result.

2. The pose estimation method for high dynamic environments based on IMU data filtering according to claim 1, characterized in that, The process of setting the sliding window size and filtering the initial IMU data based on the sliding window to obtain the corresponding slope data includes: Set the slider window size parameter; The initial IMU data enters the tail of the sliding window and begins to slide sequentially, obtaining several local IMU data points; The slope data is calculated by fitting the straight lines corresponding to the local IMU data using the least squares method.

3. The pose estimation method for high dynamic environments based on IMU data filtering according to claim 1, characterized in that, The process of filtering out data from the slope data whose peak and trough widths are greater than a threshold, as the filtered data, includes: Plot the slope data based on its positive and negative values, identify peaks and troughs. A trough appears when the slope changes from positive to negative, and a peak appears when it changes from positive to negative. Calculate the index position information of all peaks and troughs. The starting positions of the peaks and troughs are determined based on the index position information, and the widths of the corresponding peaks and troughs are calculated based on the starting positions. The widths of the peaks and troughs are compared with preset thresholds, and peaks and troughs with widths greater than the thresholds are selected as filter data. The time length within the range of peaks and troughs is used as the width.

4. The pose estimation method for high dynamic environments based on IMU data filtering according to claim 1, characterized in that, The process of performing anomaly detection on the filtered data, smoothing the abnormal data, and obtaining optimized IMU data includes: Calculate the quantiles of the initial IMU data range; Anomaly detection is performed based on the extreme values ​​of peaks and troughs, and results that exceed the range of the initial IMU data are selected as outlier data. For outlier data, replace it with the value of the adjacent normal data point to obtain smoothed data; By integrating smoothed data with data that has passed anomaly detection, optimized IMU data is obtained.

5. The pose estimation method for high dynamic environments based on IMU data filtering according to claim 1, characterized in that, The process of inputting the initial IMU data and optimized IMU data into the radar inertial odometry after secondary saturation filtering to calculate the pose estimation results includes: Saturation is calculated for the initial IMU data and the optimized IMU data respectively to obtain saturation index. The saturation index exceeding the threshold is used as the initial saturated IMU data and the optimized saturated IMU data. The initial saturated IMU data and the optimized saturated IMU data are input into the radar inertial odometer; The pose of an object is estimated using Kalman filtering to obtain the pose estimation result.

6. The pose estimation method for high dynamic environments based on IMU data filtering according to claim 1, characterized in that, The process of detecting initial IMU data for bumpy road sections includes: Error data from IMU data on bumpy road sections is detected, and after deleting the error data, the data is arranged in chronological order to obtain preprocessed data; The pitch angular velocity data is extracted from the preprocessed data and used as the initial IMU data.

7. The pose estimation method for high dynamic environments based on IMU data filtering according to claim 3, characterized in that, The process of comparing the width of each peak and trough with a preset threshold and filtering out peaks and troughs with widths greater than the threshold as the filtered data includes: If the width of the crest and trough is 0, the starting position is reset to the current time; If the width of the peak and trough is less than the threshold, it is ignored and the starting position remains unchanged; Continue testing the slope data until the filter width is greater than the threshold, at which point the test ends.

8. A pose estimation system for high-dynamic environments based on IMU data filtering, characterized in that, include: A sliding window unit is configured to detect initial IMU data on bumpy road sections. The sliding window size is set, and the initial IMU data is filtered based on the sliding window to obtain the corresponding slope data. The filtering unit is configured to filter out data in the slope data whose peak and trough widths are greater than a threshold, and use this data as the filtered data. The smoothing unit is configured to perform anomaly detection on the filtered data and smooth the abnormal data to obtain optimized IMU data. The secondary filtering unit is configured to perform secondary saturation filtering on the initial IMU data and the optimized IMU data before inputting them into the radar inertial odometry to calculate the pose estimation result.

9. An unmanned vehicle, comprising: At least one processor; And a memory storing a computer program executable on the processor, characterized in that, when the processor executes the program, it performs the steps of the pose estimation method for a high dynamic environment based on IMU data filtering as described in any one of claims 1 to 7.

10. A computer-readable storage medium / a computer program product, characterized in that, The computer-readable storage medium stores computer-executable instructions, which, when executed by a processor, perform the pose estimation method for a high-dynamic environment based on IMU data filtering as described in any one of claims 1 to 7; And / or, The computer program includes a computer program that, when executed by a processor, implements the pose estimation method for a high dynamic environment based on IMU data filtering as described in any one of claims 1 to 7.

Images