Search

CN-121977544-A - Pose estimation method and system of high dynamic environment based on IMU data screening

CN121977544ACN 121977544 ACN121977544 ACN 121977544ACN-121977544-A

Abstract

The invention provides a pose estimation method and a pose estimation system of a high dynamic environment based on IMU data screening, and relates to the field of automatic driving, wherein the method comprises the steps of detecting initial IMU data of a bumpy road section, setting the size of a sliding window, and screening the initial IMU data based on the sliding window to obtain corresponding slope data; the method comprises the steps of selecting data with the width of a wave crest and a wave trough larger than a threshold value from slope data as screening data, carrying out anomaly detection on the screening data, carrying out smoothing treatment on the anomaly data to obtain optimized IMU data, inputting initial IMU data and 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 of failure of radar inertia on the data of the unmanned vehicle passing through the deceleration strip and the bumpy road surface is solved, and the map with high local consistency is obtained, so that the influence of a local bumpy road section on the odometer is eliminated.

Inventors

  • ZHAO KANG

Assignees

  • 新石器慧通（北京）科技有限公司

Dates

Publication Date
20260505
Application Date
20260211

Claims (10)

  1. 1. The pose estimation method of the high dynamic environment based on IMU data screening is characterized by comprising the following steps of: detecting initial IMU data of a bumpy road section, setting the size of a sliding window, and screening the initial IMU data based on the sliding window to obtain corresponding slope data; Screening data with the width of the wave crest and the wave trough larger than a threshold value from the slope data as screening data; Performing anomaly detection on the screened data, and performing smoothing treatment on the anomaly data to obtain optimized IMU data; and (3) carrying out secondary saturation screening on the initial IMU data and the optimized IMU data, inputting the secondary saturation screening data into a radar inertial odometer, and calculating to obtain a pose estimation result.
  2. 2. The method for estimating the pose of the high dynamic environment based on IMU data screening according to claim 1, wherein the step of setting the size of the sliding window, screening the initial IMU data based on the sliding window, and obtaining the corresponding slope data comprises the steps of: setting a sliding window size parameter; initial IMU data enter the tail part of the sliding window to start sliding in sequence, so that a plurality of local IMU data are obtained; fitting straight lines corresponding to the local IMU data through a least square method, and calculating slope data.
  3. 3. The method for estimating the pose of the high dynamic environment based on IMU data screening according to claim 1, wherein the process of screening the slope data for data having the width of the peak and the trough greater than the threshold value comprises: identifying wave crests and wave troughs according to positive and negative drawing of slope data, and counting index position information of all wave crests and wave troughs when the slope changes from positive to negative, namely the wave troughs appear, otherwise, the wave crests appear; Determining the initial positions of the wave crests and the wave troughs based on the index position information, and calculating to obtain the widths of the corresponding wave crests and the corresponding wave troughs according to the initial positions; Comparing the widths of the wave crests and the wave troughs with a preset threshold value, and screening the wave crests and the wave troughs with widths larger than the threshold value as screening data; wherein the time length in the range of the peaks and the valleys is taken as the width.
  4. 4. The method for estimating the pose of the high dynamic environment based on IMU data screening according to claim 1, wherein the process of performing anomaly detection on the screened data and performing smoothing on the anomaly data to obtain optimized IMU data comprises the following steps: Calculating the fractional number of the measuring range of the initial IMU data; performing anomaly detection according to the extreme values of the wave crest and the wave trough, and screening a quantile result exceeding the range of the initial IMU data as anomaly data; For abnormal data, replacing the abnormal data with values of adjacent normal data points to obtain smooth data; and integrating the smooth data with the data passing through the anomaly detection to obtain optimized IMU data.
  5. 5. The pose estimation method of high dynamic environment based on IMU data screening of claim 1, wherein the process of inputting the initial IMU data and the optimized IMU data to the radar inertial odometer after secondary saturation screening and calculating to obtain the pose estimation result comprises the following steps: Respectively calculating saturation of the initial IMU data and the optimized IMU data to obtain a saturation index, and taking the saturation index exceeding a threshold value as the initial saturated IMU data and the optimized saturated IMU data; inputting initial saturated IMU data and optimized saturated IMU data into a radar inertial odometer; And estimating the pose of the object through Kalman filtering to obtain a pose estimation result.
  6. 6. The method for estimating the pose of the high dynamic environment based on IMU data screening according to claim 1, wherein the process of detecting the initial IMU data of the bumpy road section comprises the steps of: detecting error data of IMU data of a bumpy road section, and after deleting the error data, arranging according to a time sequence to obtain preprocessed data; angular velocity data in the pitch direction is extracted from the preprocessed data as initial IMU data.
  7. 7. The method for estimating the pose of the high dynamic environment based on IMU data screening according to claim 3, wherein the step of comparing the width of each peak and trough with a preset threshold value, and screening the peak and trough with a width greater than the threshold value, comprises the steps of: If the width of the wave crest and the wave trough is 0, reconstructing the starting position as the current time; if the width of the peaks and valleys is less than the threshold, ignoring and leaving the starting position unchanged; the detection of slope data is continued until the result of the screening width being greater than the threshold value is taken as the termination position.
  8. 8. The pose estimation system of the high dynamic environment based on IMU data screening is characterized by comprising the following components: The sliding window unit is configured to be used for detecting initial IMU data of a bumpy road section, setting the size of the sliding window, screening the initial IMU data based on the sliding window, and obtaining corresponding slope data; A screening unit configured to screen out data with widths of peaks and valleys larger than a threshold value from the slope data as screening data; The smoothing unit is configured to perform abnormality detection on the screening data, and perform smoothing processing on the abnormal data to obtain optimized IMU data; And the secondary screening unit is configured to screen the initial IMU data and the optimized IMU data for secondary saturation, input the filtered data into the radar inertial odometer and calculate to obtain a pose estimation result.
  9. 9. An unmanned vehicle, comprising: and a memory storing a computer program executable on the processor, wherein the processor performs the steps of the IMU data screening-based pose estimation method of a high dynamic environment according to any of claims 1 to 7 when the program is executed.
  10. 10. A computer-readable storage medium/a computer program product, characterized in that, The computer readable storage medium stores computer executable instructions that, when executed by a processor, perform the method of pose estimation for high dynamic environments based on IMU data screening as claimed in any one of claims 1 to 7, and/or, The computer program product comprises a computer program which, when executed by a processor, implements the method for estimating the pose of the high dynamic environment based on IMU data screening according to any of claims 1 to 7.

Description

Pose estimation method and system of high dynamic environment based on IMU data screening Technical Field The invention relates to the technical field of automatic driving, in particular to a pose estimation method and a pose estimation system of a high dynamic environment based on IMU data screening. Background The scale of the automatic driving mass production is continuously expanded, and the degree of dependence of the L4 level automatic driving on the map is more obvious. The accurate and robust three-dimensional reconstruction algorithm plays an important role in key links such as light map production, perception model labeling and the like. Currently, a synchronous positioning and map building (SLAM) system based on laser point cloud is a main technical method in the field of three-dimensional reconstruction by virtue of high precision and small influence of illumination. However, in practical applications, the currently prevailing radar inertia algorithms are found to be significantly inadequate in high dynamic environments. In high dynamic scenes such as deceleration strips and bumpy roads, the algorithm is difficult to perform satisfactorily, and the situation of failed drawing frequently occurs, so that the production efficiency is seriously hindered. Because under high dynamic conditions, inertial Measurement Unit (IMU) data can cause dramatic changes in the position and attitude of the odometer, thereby severely affecting the smoothness of the trajectory. Disclosure of Invention Aiming at the problems existing in the prior art, the invention provides a pose estimation method and a pose estimation system for a high dynamic environment based on IMU data screening, and an IMU data screening mechanism based on a sliding window and a slope method, so that a radar inertia algorithm can acquire stable pose estimation in the high dynamic environment. In order to achieve the above purpose, the technical scheme adopted by the invention is as follows: detecting initial IMU data of a bumpy road section, setting the size of a sliding window, and screening the initial IMU data based on the sliding window to obtain corresponding slope data; Screening data with the width of the wave crest and the wave trough larger than a threshold value from the slope data as screening data; Performing anomaly detection on the screened data, and performing smoothing treatment on the anomaly data to obtain optimized IMU data; and (3) carrying out secondary saturation screening on the initial IMU data and the optimized IMU data, inputting the secondary saturation screening data into a radar inertial odometer, and calculating to obtain a pose estimation result. In some embodiments, the setting the sliding window size, filtering the initial IMU data based on the sliding window, and obtaining the corresponding slope data includes: setting a sliding window size parameter; initial IMU data enter the tail part of the sliding window to start sliding in sequence, so that a plurality of local IMU data are obtained; fitting straight lines corresponding to the local IMU data through a least square method, and calculating slope data. In some embodiments, the process of screening the slope data for data with the width of the peak and the trough greater than the threshold value includes: identifying wave crests and wave troughs according to positive and negative drawing of slope data, and counting index position information of all wave crests and wave troughs when the slope changes from positive to negative, namely the wave troughs appear, otherwise, the wave crests appear; Determining the initial positions of the wave crests and the wave troughs based on the index position information, and calculating to obtain the widths of the corresponding wave crests and the corresponding wave troughs according to the initial positions; Comparing the widths of the wave crests and the wave troughs with a preset threshold value, and screening the wave crests and the wave troughs with widths larger than the threshold value as screening data; wherein the time length in the range of the peaks and the valleys is taken as the width. In some embodiments, the process of performing anomaly detection on the filtered data and performing smoothing on the anomaly data to obtain the optimized IMU data includes: Calculating the fractional number of the measuring range of the initial IMU data; performing anomaly detection according to the extreme values of the wave crest and the wave trough, and screening a quantile result exceeding the range of the initial IMU data as anomaly data; For abnormal data, replacing the abnormal data with values of adjacent normal data points to obtain smooth data; and integrating the smooth data with the data passing through the anomaly detection to obtain optimized IMU data. In some embodiments, the process of obtaining the pose estimation result by inputting the secondary saturation screening of the initial IMU data and the optimized IMU