A method for fusing odometer and IMU dead reckoning based on ground classification

Abstract

This invention discloses a method for odometry fusion IMU track estimation based on ground classification. To overcome the problem of inaccurate positioning by existing 2D LiDAR in overly open indoor / outdoor environments or in corridors and tunnels with few features, this invention includes: collecting basic data in a preset environment and setting corresponding ground labels; training a random forest multi-classification model using the basic data and ground labels; setting optimization variables; processing the basic data and inputting it into an extended Kalman filter model; using minimum mean square error as the objective function; offline optimization to obtain the coefficients of the optimal extended Kalman filter prediction model based on each preset environment; and using the optimal coefficients and the trained random forest multi-classification model, calculating the ground category online during navigation and dynamically adjusting it based on the optimal coefficients to achieve an adaptive extended Kalman filter effect.

Description

Technical Field

[0001] This invention relates to the field of robot localization, and more particularly to a method for calculating odometry-integrated IMU tracks based on ground classification. Background Technology

[0002] Localization is a crucial issue in robot SLAM and navigation, and is one of the prerequisites for realizing robot intelligence. Currently, for indoor navigation robots, the more commonly used localization methods include lasers, depth cameras, IMUs, and wheel odometry. The first two determine the robot's position by detecting the environment, while the latter two monitor the robot's own motion data, infer the robot's relative displacement over time, and calculate the final pose.

[0003] In the unique environment of long corridors, due to the similarity of environmental information, feature point matching based on lidar has a large amount of repetitive information, which easily leads to positioning drift or even positioning errors. In relatively open indoor and outdoor environments, due to the limited detection range of 2D lidar, there are often few feature points, and relying on feature point matching in this case can easily lead to matching failure or even matching errors. In addition, due to cost constraints, 2D planar lidar is a commonly used radar. It often only scans the contour features on a plane at a fixed height in the environmental space, and the amount of information is relatively simple, leaving considerable room for improvement. 3D lidar is more expensive, but its detection range is very large, reaching 50-100m. It plays a crucial role in 3D SDA AM, which can obtain richer environmental information. At the same time, there are very mature algorithms for trajectory estimation, with an average error of about 1-2cm, which can be used as the true value in the parameter optimization process of this invention.

[0004] A method for indoor robot positioning, disclosed in Chinese patent literature (publication number CN106643694B), includes a data storage step for storing encoder data and ultra-wideband (UWB) data from the previous moment; a data acquisition step for acquiring encoder data and UWB data from the current moment via photoelectric encoders mounted on the shafts of motors driving the robot's left and right wheels; an incremental data calculation step for calculating mileage and position increments based on the stored encoder and UWB data from the previous moment and the acquired encoder and UWB data from the current moment; and a positioning information evaluation step for determining whether the encoder and UWB data are abnormal based on the calculated mileage and position increments, thereby determining whether to use an unscented Kalman filter (UKF) method, dead reckoning method, or UWB pose information obtained from the UWB positioning system to obtain the robot's positioning information at the current moment. However, the overall computational load required by this solution is relatively large, thus exhibiting certain drawbacks. Summary of the Invention

[0005] This invention primarily addresses the problem of inaccurate positioning of existing 2D lidar in overly open indoor or outdoor environments, or in corridors and tunnels with few features. It provides a ground-based odometry fusion IMU trajectory estimation method, which can achieve robot-assisted positioning in special corridor environments.

[0006] The above-mentioned technical problems of the present invention are mainly solved by the following technical solutions:

[0007] This invention includes: collecting basic data and setting corresponding ground labels in a preset environment; training a random forest multi-classification model using the basic data and ground labels; setting optimization variables, processing the basic data, and inputting it into an extended Kalman filter model; using minimum mean square error as the objective function to offline optimize the coefficients of the optimal extended Kalman filter prediction model based on each preset environment; and using the optimal coefficients and the trained random forest multi-classification model to calculate the ground category online during navigation, and dynamically adjusting the model based on the optimal coefficients. Different preset environments, i.e., different ground environments, can avoid positioning drift and positioning errors caused by incorrect LiDAR information by comprehensively analyzing the optimal coefficients and the Kalman filter model.

[0008] Preferably, the preset environment includes, but is not limited to, indoor wood flooring, indoor ceramic tiles, indoor floor tiles, indoor carpet, damp indoor wood flooring, damp indoor ceramic tiles, damp indoor floor tiles, damp indoor carpet, outdoor asphalt, outdoor cement, outdoor floor tiles, damp outdoor asphalt, damp outdoor cement, or damp outdoor floor tiles; the basic data includes: odometer readings and real-time speed, IMU real-time acceleration and angular velocity, real-time estimated pose obtained through 3D LiDAR SLAM, and corresponding timestamps.

[0009] Preferably, the collected basic data and corresponding ground labels are cropped to remove useless data, including static states. The cropped basic data is then aligned with the timestamps and merged. The data merging process includes selecting data at 500ms intervals and merging them into a set of states, which are then input into the random forest classification model.

[0010] Preferably, the odometry and real-time velocity, IMU real-time acceleration and angular velocity after random forest processing are sampled row by row and column by column with replacement; several decision trees are built for the sampled data samples, and the decision trees include two states: in the first state, no child node of the decision tree can continue to split; in the second state, all samples in the decision tree point to the same ground classification; when predicting a sample, the prediction results of each decision tree in the random forest for that sample are counted, and the best result is selected from all prediction results by voting.

[0011] Preferably, the basic data is input into the extended Kalman filter model, and the prediction model coefficients, odometer readings, real-time speed, and IMU variance are set as optimization variables. The minimum mean square error is used as the objective function to optimize the coefficients of the optimal extended Kalman filter prediction model based on different preset environments offline. The prediction model is optimized based on the extended Kalman filter, and a predicted pose is generated after each filtering. The sum of the mean square errors between the predicted pose and the true pose is used as the optimization objective.

[0012] As a preferred method, the real-time estimated pose with timestamps is obtained by 3D LiDAR SLAM, the difference between the real-time estimated pose and the pose obtained by odometry is calculated, the timestamps of the real-time estimated pose are aligned, and the pose after timestamp alignment is taken as the real pose.

[0013] Preferably, the navigation process involves online calculation of the preset environment category: acquiring continuously generated odometer and IMU data, collecting n data points every interval t, inputting the data into the preset environment classification model, and outputting the current preset environment category; and dynamically adjusting the robot during operation by combining the calculated preset environment category with optimal coefficients, wherein the optimal coefficients include the coefficients of the extended Kalman filter prediction model and the mean square error between the estimated pose and the actual pose.

[0014] The beneficial effects of this invention are:

[0015] 1. This invention provides a ground classification-based odometry fusion IMU trajectory estimation method, primarily applicable to overly open indoor and outdoor environments or corridors and tunnels with few features. Therefore, when ICP registration based on lidar is unreliable, positioning is achieved solely through fusion filtering of wheeled odometry and IMU. Different parameters can be automatically adjusted for different terrains, resulting in accuracy and efficiency. 2. This invention's ground classification-based odometry fusion IMU trajectory estimation method also provides better and more accurate pose estimation for lidar or visual point cloud positioning methods in ordinary environments. Furthermore, this invention has low computational cost, low upfront computation consumption, and extensive training and optimization processes are completed offline, resulting in high portability. Attached Figure Description

[0016] Figure 1 This is a diagram illustrating the offline training of a ground classification model and parameter optimization process for a ground classification-based odometry fusion IMU track extrapolation method according to the present invention.

[0017] Figure 2 This is a diagram showing the relationship between the robot's theoretical coordinate system and actual coordinate system in the global coordinate system for a ground classification-based odometry fusion IMU trajectory estimation method according to the present invention.

[0018] Figure 3This is a diagram illustrating the online pose estimation and dynamic parameter adjustment process of an odometry fusion IMU trajectory extrapolation method based on ground classification according to the present invention. Detailed Implementation

[0019] The technical solution of the present invention will be further described in detail below through embodiments and in conjunction with the accompanying drawings.

[0020] Example:

[0021] This embodiment presents a method for estimating flight paths using ground-based classification-based odometry fusion IMU, such as... Figures 1-3 As shown, the process includes: collecting data under different ground environments, including real-time odometry velocity, real-time IMU acceleration and angular velocity, real-time estimated pose obtained through 3D LiDAR SLAM, and corresponding timestamps; training a random forest multi-classification model using the collected odometry and IMU data and corresponding ground labels; using the collected odometry, IMU, and 3D LiDAR SLAM real-time estimated pose data, in the extended Kalman filter model, using the coefficients of the prediction model and the variance of the odometry and IMU as optimization variables, and using minimum mean square error as the objective function, offline optimization is performed to obtain the coefficients of the optimal extended Kalman filter prediction model based on different ground environments; by offline optimization of the optimal coefficients based on different ground environments and the offline trained random forest multi-classification model, the ground category is calculated online during navigation, and the optimal coefficients are obtained by looking up a table, achieving the effect of adaptive extended Kalman filtering.

[0022] Specifically, data is collected in different ground environments, including but not limited to indoor wood flooring, indoor ceramic tiles, indoor floor tiles, indoor carpet, damp indoor wood flooring, damp indoor ceramic tiles, damp indoor floor tiles, damp indoor carpet, outdoor asphalt, outdoor cement, outdoor floor tiles, damp outdoor asphalt, damp outdoor cement, damp outdoor floor tiles, etc. The ground types are set in order as categories 1, 2, 3... Data collected in these ground environments includes but is not limited to real-time velocity from the odometer, real-time acceleration and angular velocity from the IMU, real-time estimated pose obtained through 3D LiDAR SLAM, and corresponding timestamps.

[0023] Specifically, the method for processing the training data of the random forest multi-classification model is as follows: using the collected odometer and IMU data, and the corresponding ground truth labels, the data is cropped, and the useless data including the stationary state is manually removed. The data is aligned in terms of timestamps, and the alignment method is interpolation. Since the odometer frequency is generally 20hz and the IMU frequency is generally 100hz, to ensure the accuracy of the predicted pose during the offline optimization process, the odometer data is aligned with the IMU timestamps, that is, the odometer data is interpolated, and then the data is merged, including selecting the data within every 500ms, and the merged data is used as a set of states and input into the classification model of the random forest.

[0024] Specifically, the method for training the random forest multi-classification model is as follows: First, there are two random sampling processes. The random forest needs to perform row and column sampling on the processed odometer and IMU data. For row sampling, it is done with replacement, that is, in the sampled sample set, there may be duplicate samples. The decision tree is built using the sampled data in this way. So, for a certain leaf node of the decision tree, either it cannot be further split, or all the samples in it point to the same classification. The random forest constructs multiple decision trees. When predicting a certain sample, the prediction results of each tree in the forest for this sample are counted, and then the final result is selected from these prediction results through the voting method.

[0025] Specifically, the growth method of each tree in the forest is as follows:

[0026] If there are N samples in the training samples, then sample N times with replacement from these N samples, and use the obtained samples to build the tree; Let M be the number of features of the input samples. When splitting each node, we first select m (m << M) features from these M features, and then select the best split point among these m features for splitting; Each tree grows as much as possible without pruning.

[0027] Specifically, the prediction process is as follows:

[0028]

[0029]

[0030] Specifically, the update process is as follows:

[0031]

[0032]

[0033]

[0034] Specifically, using real-time pose estimation data from odometry, IMU, and 3D LiDAR SLAM, the coefficients of the prediction model and the variances of the odometry and IMU are used as optimization variables (ABQR) in the extended Kalman filter model, with the minimum mean square error as the objective function. The coefficients of the optimal extended Kalman filter prediction model based on different ground environments are optimized offline. The optimization model is based on the extended Kalman filter and optimizes the prediction model coefficients and observation variance. After each filtering, an estimated pose is obtained. The sum of the mean square error between the estimated pose and the true pose is used as the optimization objective.

[0035] Specifically, the method for obtaining the true pose is as follows: first, the real-time estimated pose obtained by 3D LiDAR SLAM with a timestamp is interpolated with the pose obtained by odometry, that is, the timestamp of the pose estimated by 3D LiDAR is aligned with the timestamp of the predicted pose, and the pose after timestamp alignment is used as the true pose input into the optimization process.

[0036] Specifically, the method for calculating the ground category online during navigation is as follows: by continuously generating odometry and IMU data, topic data is acquired, and n-dimensional data is collected every time t. The data is then input into the ground classification model, and the current ground category is output.

[0037] Specifically, the optimal coefficients are obtained by looking up the calculated ground category, including the coefficients and observation variance of the extended Kalman filter prediction model, and dynamically adjusted during the movement to achieve the effect of adaptive extended Kalman filtering.

[0038] It should be understood that the embodiments are for illustrative purposes only and are not intended to limit the scope of the invention. Furthermore, it should be understood that after reading the teachings of this invention, those skilled in the art can make various alterations or modifications to the invention, and these equivalent forms also fall within the scope defined by the appended claims.

Claims

1. A ground classification based odometer fusion IMU dead reckoning method, characterized in that, Comprise: S1: Collecting basic data in a preset environment and setting corresponding ground labels, training a random forest multi-classification model through the basic data and the ground labels; the basic data comprises odometer and real-time speed, IMU real-time acceleration and angular velocity, real-time estimated pose obtained through 3D laser radar SLAM and corresponding timestamp; S2: Setting optimization variables, including prediction model coefficients, odometer and real-time speed and IMU variance; after processing the basic data, bringing it into an extended Kalman filter model, using the least mean square error of the estimated pose and the real pose as the objective function, offline optimizing the coefficients of the optimal extended Kalman filter prediction model based on each preset environment; S3: Through the optimal coefficients and the trained random forest multi-classification model, online calculating the ground category in the navigation process, and dynamically adjusting combined with the optimal coefficients.

2. The method of claim 1, wherein, S1.1.1: The preset environment comprises but is not limited to indoor wooden floor, indoor ceramic tile, indoor tile, indoor carpet, wet indoor wooden floor, wet indoor ceramic tile, wet indoor tile, wet indoor carpet, outdoor asphalt, outdoor cement, outdoor tile, wet outdoor asphalt, wet outdoor cement or wet outdoor tile.

3. The method of claim 1, wherein, S1.2.1: Data clipping is performed on the collected basic data and corresponding ground labels, and useless data including stationary state is removed, the clipped basic data is aligned with the timestamp, and then data merging is performed; the data merging process comprises selecting data in each 500ms time interval, merging as a group of state input into the classification model of the random forest.

4. The odometer fusion IMU dead reckoning method based on ground classification according to claim 1 or 2 or 3, characterized in that, S1.2.2: The odometer and real-time speed, IMU real-time acceleration and angular velocity processed by the random forest are sampled row by row and column by column with replacement; S1.2.2: a plurality of decision trees are established for the sampled data samples, the decision trees comprise two states, in the first state, any sub-node of the decision tree cannot continue to split, and in the second state, all samples in the decision tree point to the same ground classification; S1.2.3: when predicting a sample, the prediction results of each decision tree in the random forest for the sample are counted, and the best result is selected from all the prediction results by voting.

5. The odometer fusion IMU dead reckoning method based on ground classification according to claim 4, characterized in that, S2.1.1: Bringing the basic data into the extended Kalman filter model, setting the prediction model coefficients, odometer and real-time speed and IMU variance as optimization variables; S2.1.2: Taking the least mean square error as the objective function, offline optimizing the coefficients of the optimal extended Kalman filter prediction model based on different preset environments; S2.1.3: the prediction model is based on the optimization of the extended Kalman filter, and an estimated pose is generated after each filtering, and the sum of the mean square errors of the estimated pose and the real pose is used as the optimization target.

6. The odometer fusion IMU dead reckoning method based on ground classification according to claim 5, characterized in that, S2.2: Obtaining real-time estimated pose with timestamp through 3D laser radar SLAM, performing difference value between the real-time estimated pose and the pose obtained by the odometer, aligning the timestamps of the real-time estimated pose and the estimated pose, and using the timestamp-aligned pose as the real pose.

7. The odometer fusion IMU dead reckoning method based on ground classification according to claim 1 or 6, characterized in that, S3.1: online computing preset environment category in navigation process: acquiring continuously generated odometer and IMU data, collecting n-dimensional data every interval t, inputting the data into a preset environment classification model, and outputting the current preset environment category; S3.2: dynamically adjusting the robot during operation by combining the calculated preset environment category and the optimal coefficient, wherein the optimal coefficient includes the coefficient of the extended Kalman filter prediction model and the mean square error of the estimated pose and the real pose.

Images