Unmanned mine car attitude calculation method based on laser radar and IMU fusion

Abstract

The invention relates to the field of automatic driving, in particular to a laser radar and IMU fusion-based unmanned mine car pose calculation method, which comprises the following steps of: calculating an estimated pose of an unmanned mine car based on point cloud data; acquiring sensor data of an IMU and a speedometer; generating map information based on the point cloud data and the sensor data; calculating the zero deviation of the gyroscope according to the zero deviation estimation target function of the gyroscope, and correcting the data of the sensor; based on a sliding window, solving a predefined cost function, and optimizing the estimated pose and the corrected sensor data to obtain an optimized state vector; carrying out closed-loop detection; and when a closed loop appears, correcting all the optimized state vectors by solving a preset error function according to the optimized state vectors to obtain the real pose of the unmanned mine car. According to the invention, the limitation of single laser radar positioning in a feature-deficient or repetitive environment can be overcome, and the positioning accuracy is improved.

Description

Technical Field

[0001] The present invention relates to the field of autonomous driving, and particularly to a method for calculating the pose of an unmanned mining vehicle based on the fusion of lidar and IMU. Background Art

[0002] High-precision navigation and positioning is an important foundation for an unmanned mining vehicle in an open-pit mine to perform various tasks. It can accurately provide the position and pose information of the unmanned mining vehicle in a complex environment, and then guide the movement of the unmanned mining vehicle to ensure its safe and accurate completion of tasks. In an open-pit mine, due to the complex and changeable terrain and harsh environment, a high-precision navigation and positioning system is particularly important for avoiding collisions, ensuring the precise control and efficient operation of the mining vehicle.

[0003] Currently, lidar is a commonly used technology in the positioning of unmanned mining vehicles. This technology emits and receives laser beams, measures the distance to objects in the environment, and generates high-precision three-dimensional point cloud data. By processing this data, the relative pose of the unmanned mining vehicle can be estimated. However, the pose estimation of lidar mainly depends on the features of the environment. Therefore, in an environment with scarce features or repetitive environments, errors will occur in the positioning results, resulting in low positioning accuracy. Summary of the Invention

[0004] To solve the above problems, the present invention provides a method for calculating the pose of an unmanned mining vehicle based on the fusion of lidar and IMU.

[0005] The method for calculating the pose of an unmanned mining vehicle based on the fusion of lidar and IMU includes:

[0006] Step 1, calculating the estimated pose of the unmanned mining vehicle based on the point cloud data of the lidar;

[0007] Step 2, obtaining the sensor data of the IMU and the odometer;

[0008] Step 3, generating map information based on the point cloud data and the sensor data;

[0009] Step 4, defining a gyroscope zero-bias estimation objective function, calculating the gyroscope zero-bias based on the estimated pose and the sensor data according to the gyroscope zero-bias estimation objective function, and correcting the sensor data according to the gyroscope zero-bias to obtain the corrected sensor data;

[0010] Step 5, solving a predefined cost function based on a sliding window, and optimizing the estimated pose and the corrected sensor data to obtain an optimized state vector;

[0011] Step 6, performing loop detection based on the map information and the point cloud data;

[0012] Step 7: When a closed loop occurs, according to the optimized state vector, by solving the preset error function, all optimized state vectors are corrected to obtain the true pose of the driverless mining truck.

[0013] Further, the steps of calculating the estimated pose of the driverless mining truck based on the point cloud data of the lidar include:

[0014] Classify the point cloud data through a deep learning neural network, calculate the curvature of each point in the point cloud data, and extract feature points;

[0015] Transfer the feature points of the front and rear frames to the same coordinate system for matching to find the correlation of feature points between consecutive frames;

[0016] Calculate the estimated pose of the driverless mining truck according to the correlation of feature points between consecutive frames.

[0017] Further, the steps of Step 2 include:

[0018] Denoise the measurement values of the IMU;

[0019] Integrate the denoised measurement values;

[0020] Perform error correction on the integration result;

[0021] Combine the data after error correction with the measurement information of the odometer to obtain sensor data.

[0022] Further, the steps of Step 3 include:

[0023] Obtain the motion scale information of the driverless mining truck through the sensor data;

[0024] Fuse the continuous point cloud data according to the estimated pose and the motion scale information to obtain map information.

[0025] Further, the steps of defining the gyroscope zero bias estimation objective function include:

[0026] The gyroscope zero bias estimation objective function is:

[0027]

[0028] Among them, b ω represents the zero bias drift of the accelerometer gyroscope; δ represents the error; and Obtain the external parameter rotation by combining lidar SFM with lidar and IMU; is the estimated value of rotation pre-integration; is the Jacobian matrix of the pre-integrated rotation γ with respect to the gyroscope zero bias b ω indicating δbω How does a small change in k affect the value of γ? k represents the serial number of the scan frame of the lidar, N represents the total number of scans of the lidar, γ represents the rotation pre-integration term of the IMU, and b 0 represents the zero-bias drift of the accelerometer gyroscope in the body coordinate system of the k-th frame, and c represents the intersection, ‖‖ 2 represents the norm, which is a mapping from a linearly normed space to non-negative real numbers.

[0029] Furthermore, the optimized state vector in step five is:

[0030]

[0031] where χ represents the optimized state vector, and x k represents the point cloud of the n+1-th frame x 0 , x 1 , …, x n , and any one of them, λ m represents the inverse depth of the (m+1)-th feature point. The optimized state vector includes the IMU position attitude velocity gyro zero bias b g , accelerometer zero bias b a , lidar-IMU extrinsic rotation and translation IMU-odometry extrinsic rotation and translation

[0032] Furthermore, the specific steps of step six include:

[0033] Cluster the point cloud data;

[0034] Remove the ground from the clustered point cloud data to obtain the segmented object;

[0035] For each segmented object, use deep learning methods to extract its features;

[0036] Based on the KD-tree algorithm, perform feature matching between the newly collected point cloud data and the already stored point cloud data;

[0037] Use the random sample consensus method to judge the consistency of the matching results. Only when the matching results pass the consistency verification, it is determined that a loop closure occurs.

[0038] Furthermore, the steps of step seven include:

[0039] When a closed loop appears, a pose graph is created based on the optimized state vector. Each node in the pose graph represents a pose in the optimized state vector, and each edge represents a preset observation value. By solving the preset error function, all optimized state vectors are corrected to obtain the true pose of the unmanned mining vehicle.

[0040] One or more technical solutions provided in the embodiments of the present invention have at least the following technical effects or advantages:

[0041] The present invention proposes a method for calculating the pose of an unmanned mining vehicle based on the fusion of lidar and IMU. By fusing the data of lidar and IMU, the limitations of single lidar positioning in feature-poor or repetitive environments can be overcome, and the positioning accuracy can be improved. At the same time, this method also uses technologies such as sliding window optimization and loop closure detection to further improve the accuracy and robustness of pose estimation. Generally speaking, this method can provide more accurate and stable unmanned driving mining vehicle pose information in complex open-pit mine environments, thereby improving the operation efficiency and safety of the mining vehicle. Description of the Drawings

[0042] Figure 1 It is a schematic flow chart of a method for calculating the pose of an unmanned mining vehicle based on the fusion of lidar and IMU provided by an embodiment of the present invention. Detailed Embodiments

[0043] The following will describe the present invention in detail with reference to the drawings and specific embodiments. Before elaborating on the technical solutions of the embodiments of the present invention, the nouns and terms involved will be explained. In this specification, components with the same name or the same reference numeral represent similar or the same structures, and are for illustrative purposes only.

[0044] The method steps of the present invention are as Figure 1 shown and are specifically explained as follows:

[0045] 1. Calculate the pose from point cloud data

[0046] The goal of calculating the pose from point cloud data is to find the same or similar feature points in continuous lidar scans, and estimate the attitude and position of the unmanned mining vehicle, that is, the pose, through the matching between these feature points. The accurate estimation of the pose directly affects the navigation accuracy and safety of the unmanned mining vehicle.

[0047] The point cloud data of the lidar of the unmanned mining vehicle includes the distance information, azimuth information, intensity information, color information, number of echoes, speed information, point cloud data, data timestamp, and data quality identification data of each three-dimensional space point.

[0048] Classify the point cloud data through a deep learning neural network, calculate the curvature of each point, and extract feature points. These feature points include terrain features such as edges and corners, which are all key information helpful for pose estimation.

[0049] After extracting the feature points, transform the feature points to the coordinate system of the current frame moment, and then transform the feature points of the front and rear frames to the same coordinate system for matching to find the correlation of the feature points between consecutive frames, thereby estimating the pose of the unmanned mining vehicle. The matching method can be to calculate the similarity between features or use some machine learning methods to determine whether the features match.

[0050] Calculate the pose of the unmanned mining vehicle through the matching of the feature points of the front and rear frames.

[0051] 2. IMU / Odometer Data Preprocessing

[0052] The data of the IMU (Inertial Measurement Unit) contains the specific force, angular velocity, and direction angle of the vehicle, while the odometer measures the distance traveled by the vehicle. Although the measurement contents of the IMU and the odometer are different, they jointly provide the motion state of the unmanned mining vehicle.

[0053] Since the measurement rate of the IMU is much faster than that of the lidar, directly processing the high-rate IMU data will consume a large amount of computing resources. Therefore, the present invention uses the method of pre-integration to process the IMU data. Pre-integration is a data processing method based on integration, which can convert the high-frequency IMU data into low-frequency velocity or position data. In this way, subsequent data processing does not need to process a large amount of high-frequency data, thereby reducing the consumption of computing power.

[0054] The process of pre-integration includes noise processing of the measurement values of the IMU, integration of the processed measurement values, and finally error correction of the integration results. In this process, various filters and numerical integration methods can be used, such as the Kalman filter and the Euler method.

[0055] After processing the IMU data, add its measurement information to the odometer to further improve the accuracy of the pose estimation of the unmanned mining vehicle, obtain the increments of the attitude, speed, and position of the unmanned mining vehicle, and provide accurate basic data for subsequent navigation and path planning.

[0056] 3. Lidar SFM

[0057] The goal of this step is to restore the motion state of the unmanned mining vehicle and build an accurate environmental map by processing the lidar data to support the positioning and navigation of the unmanned mining vehicle.

[0058] During the navigation of an unmanned mining vehicle, the lidar continuously scans the environment, generating a series of point cloud data that contains environmental information and the movement information of the unmanned mining vehicle. By processing this data and based on some inherent properties of the lidar itself, such as focal length, pixel size, lens distortion, etc., and the position and orientation of the lidar relative to the vehicle body coordinate system, the reference trajectory of the unmanned mining vehicle in the lidar coordinate system can be obtained, which reflects the movement state of the unmanned mining vehicle.

[0059] However, this reference trajectory only shows the relative movement of the unmanned mining vehicle and does not reflect its movement in the real world. To obtain the real movement trajectory, the movement scale information is needed, that is, the real distance the unmanned mining vehicle moves in each step. This information can be obtained through other sensors (such as IMU, odometer, etc.).

[0060] After having the real-scale movement trajectory, the real scale of the map can be determined. The map is obtained by aligning and fusing continuous point cloud data. The alignment and fusion in this process rely on the movement trajectory of the unmanned mining vehicle. Therefore, knowing the real scale of the movement trajectory can determine the real scale of the map.

[0061] 4. Alignment of Lidar, IMU and Odometer

[0062] Lidar data provides the accurate position information of the unmanned mining vehicle in the environment. However, due to the usually low update frequency of the lidar, it cannot provide high-frequency movement state information. On the contrary, IMU and odometer can provide high-frequency movement state information, including acceleration and angular velocity. However, due to the influence of various factors (such as device noise, temperature difference, power supply change, etc.), they often have errors. Especially after long-term operation, these errors may accumulate, resulting in an increase in the deviation of pose estimation.

[0063] To solve this problem, it is necessary to align and fuse these data. First, based on the data of IMU and odometer, as well as lidar data, the bias of the gyroscope is estimated. The gyroscope is an important part of the IMU, which can measure the angular velocity of the unmanned mining vehicle. However, due to various reasons, the readings of the gyroscope often have biases. By estimating this bias, the readings of the gyroscope can be corrected, thereby improving the measurement accuracy of the angular velocity.

[0064] The objective function for gyroscope zero bias estimation is:

[0065]

[0066] where b ω represents the zero bias drift of the accelerometer gyroscope; δ represents the error; and Obtain the extrinsic rotation by combining lidar SFM with lidar and IMU; Is the estimated value of rotation pre-integration; Is the Jacobian matrix of the pre-integrated rotation γ with respect to the gyroscope zero bias b ω Indicates how a small change in δb ω Affects the value of γ. k represents the serial number of the lidar scan frame, n represents the total number of lidar scans, γ represents the IMU rotation pre-integration term, and b k Represents the zero bias drift of the accelerometer and gyroscope in the body coordinate system at the k-th frame, and c 0 Represents the extrinsic parameters of the lidar in the odometry body coordinate system. Represents the intersection, ‖‖ 2 Represents the norm, which is a mapping from a linear normed space to non-negative real numbers.

[0067] By solving the gyroscope zero bias estimation objective function, the gyroscope zero bias δb ω Can be obtained. This estimated value will be used to correct the IMU data and odometry data, thereby improving the accuracy of pose estimation.

[0068] At the same time, recover velocity, gravity vector, and scale factor information:

[0069] Velocity recovery: By integrating the IMU data, a preliminary estimate of the velocity is obtained; using lidar data, another velocity estimate is obtained by comparing consecutive lidar scans. These two velocity estimates are fused and optimized to obtain a more accurate velocity estimate.

[0070] Gravity vector recovery: Gravity vector recovery mainly depends on IMU data. The triaxial accelerometer of the IMU measures the gravitational acceleration in a stationary state. Therefore, a preliminary estimate of the gravity vector can be obtained through the IMU readings. Then, the ground plane information obtained by the lidar can be used to further optimize the estimate of the gravity vector.

[0071] Scale factor recovery: Through IMU data, an estimate of the displacement of the unmanned mining vehicle can be obtained; at the same time, through lidar data, another displacement estimate can be obtained. Then, by comparing these two displacement estimates, the scale factor is calculated. This scale factor is used to adjust the IMU data to align it with the lidar data.

[0072] By estimating the bias of the gyroscope, the measurement accuracy of the angular velocity can be improved; by recovering the velocity, gravity vector, and scale factor information, the measurement accuracy of the motion acceleration can be improved. These will all help to obtain more accurate pose information in subsequent steps, thereby improving the navigation performance and safety of the unmanned mining vehicle.

[0073] 5. Optimize the pose estimation of the unmanned mining vehicle and sensor data fusion

[0074] During the process of lidar pose estimation, the sliding window is an important technique mainly used to reduce the computational load and improve the estimation accuracy. Especially when dealing with high-frequency IMU data, the sliding window technique can greatly improve the efficiency of data processing.

[0075] The basic idea of the sliding window is to process data within a window of a fixed length, only retaining the data within the window and discarding the data outside the window. When new data arrives, the window slides forward by one position, discarding the earliest data and receiving the new data. In this way, the amount of data processed each time is limited to a fixed range, ensuring the real-time performance and efficiency of the calculation.

[0076] In the lidar pose estimation of an unmanned mining vehicle, the sliding window contains n + 1 frames of point clouds and m + 1 feature points. These point clouds and feature points constitute the motion state and environmental characteristics of the unmanned mining vehicle.

[0077] The optimized state vector χ of the sliding window can be expressed as:

[0078]

[0079] where χ represents the state vector, x k represents any one of the n + 1 frames of point clouds x 0 , x 1 , …, x n , λ m represents the inverse depth of the (m + 1)-th feature point. The optimized state vector includes the IMU positions attitudes velocities gyro biases b g , accelerometer biases b a , lidar-IMU extrinsic rotation and translation IMU-odometry extrinsic rotation and translation

[0080] The inverse depth of a feature point refers to the reciprocal of the depth value, i.e., inverse depth. In SLAM (Simultaneous Localization and Mapping), inverse depth parameterization is widely used mainly to optimize the positions of feature points. The main advantage of inverse depth parameterization is that it can better handle distant feature points because when the feature points are far away, using inverse depth can avoid computational problems caused by overly large depth values.

[0081] The error terms of lidar data are usually composed of the registration errors of 3D point cloud data, while the error terms of IMU / odometer data are composed of the differences between the state estimates of pre-integration and the actual measurements. The optimization objective function of the sliding window is composed of error terms, including the differences between the predicted and actual values of lidar data and IMU / odometer data.

[0082] During the optimization process, some constraint conditions are set to ensure that the solution of the optimization problem conforms to physical rules. These constraint conditions include the motion model of the IMU, the geometric constraints of lidar data, and the time synchronization constraints between the IMU and the lidar.

[0083] Use a non-linear optimization algorithm, such as the Levenberg-Marquardt algorithm or the Gauss-Newton algorithm, to solve the optimization objective function of the sliding window based on the constraint conditions to obtain the optimal state vector.

[0084] The processing steps of the sliding window variables are a key link in lidar pose estimation, which plays a crucial role in improving the accuracy and real-time performance of pose estimation. Through the processing of sliding window variables, the limited computing resources can be effectively utilized to process a large amount of lidar and IMU data, thereby realizing high-precision and real-time pose estimation of unmanned mining vehicles.

[0085] 6. Closed-loop Detection and Verification Based on 3D Point Cloud Segmentation

[0086] Cumulative error refers to the error caused by factors such as sensor noise, model imperfection, and environmental changes. Compare the predicted pose and the actual pose. If the gap between the two exceeds a certain threshold, it is determined that cumulative error occurs. This step detects and identifies the places that the unmanned mining vehicle has passed through, thereby correcting the accumulated positioning error and establishing a globally consistent map.

[0087] The lidar of the unmanned mining vehicle continuously collects point cloud data of the surrounding environment, which contains a large amount of environmental information. Cluster the point cloud data and group the point clouds belonging to the same object into one cluster. The present invention does not limit the clustering method, and for example, the Euclidean clustering method based on distance can be adopted.

[0088] After obtaining the point cloud clustering, the ground is removed to obtain the segmented objects, because the features of the ground are usually relatively average and not suitable for matching. For each segmented object, deep learning methods are used to automatically extract its features. After the features are extracted, KD trees are used for fast nearest neighbor search to match the newly collected point cloud data with the point cloud data already stored in the map. The random sample consensus method (RANSAC) is used to judge the consistency of the matching results. Only when the matching results pass the consistency verification can they be considered a valid closed loop. Through closed loop detection and verification, loop information can be obtained.

[0089] Closed loop detection can correct the cumulative error in the positioning of the unmanned mining vehicle, thereby improving the positioning accuracy. Through closed loop detection and verification, the influence of environmental changes and sensor noise can be resisted to a certain extent, and the robustness of the system can be improved.

[0090] 7. Global Pose Graph Optimization Based on Map Construction

[0091] Graph optimization is used for global pose graph optimization. In this process, a pose graph is created. Each node in the pose graph represents a pose (position and orientation) in the optimized state vector, and each edge represents a preset observation value. These observation values include data obtained from sensors such as lidar and IMU / odometer, as well as loop information obtained through the closed loop detection and verification stage. For example, the measurement values of the same feature point observed from two different poses.

[0092] The optimization goal of the pose graph is to find a set of poses that minimizes the total prediction error of all observation values. This prediction error is the gap between the predicted observation value (the observation value estimated based on the current pose) and the actual observation value. The optimization problem of the pose graph is solved based on the probabilistic graph model. In the embodiments of the present invention, the factor graph model is used to solve the optimization problem of the pose graph.

[0093] After global pose graph optimization, the accuracy of pose estimation is improved, thereby improving the accuracy of the navigation and path planning of the unmanned mining vehicle, and a globally consistent pose estimation is obtained, which means that the trajectory and the map of the unmanned mining vehicle are consistent and will not deviate due to cumulative errors.

[0094] The above-described embodiments are only descriptions of the preferred embodiments of the present invention and do not limit the scope of the present invention. Without departing from the design spirit of the present invention, various deformations and improvements made by those of ordinary skill in the art to the technical solutions of the present invention shall fall within the protection scope determined by the claims of the present invention.

Claims

1. A method for calculating the posture of an unmanned mining vehicle based on the fusion of laser radar and IMU, characterized in that: The following steps are involved: Step 1: Calculate the estimated position of the unmanned mining vehicle based on the point cloud data of the LiDAR. Step 2: Get the sensor data of IMU and odometer; Step 3: generating map information based on the point cloud data and the sensor data; Step 4, defining a gyroscope zero bias estimation objective function, calculating a gyroscope zero bias according to the gyroscope zero bias estimation objective function based on the estimated posture and the sensor data, and correcting the sensor data according to the gyroscope zero bias to obtain corrected sensor data; Step 5: Based on the sliding window, solve the predefined cost function, optimize the estimated pose and the corrected sensor data, and obtain the optimized state vector; Step 6: Perform closed-loop detection based on map information and point cloud data; Step 7: When a closed loop occurs, all optimized state vectors are corrected according to the optimized state vector by solving the preset error function to obtain the true position and posture of the unmanned mining vehicle.

2. According to claim 1, the method for calculating the posture of an unmanned mining vehicle based on the fusion of laser radar and IMU is characterized in that: Based on the point cloud data of the LiDAR, the steps for calculating the estimated position of the unmanned mining vehicle include: The point cloud data is classified through a deep learning neural network, the curvature of each point in the point cloud data is calculated, and the feature points are extracted; Transfer the feature points of the previous and next frames to the same coordinate system for matching, and find out the correlation between the feature points of consecutive frames; The estimated position and posture of the unmanned mining vehicle is calculated based on the correlation of feature points between consecutive frames.

3. According to claim 1, the unmanned mining vehicle posture calculation method based on the fusion of laser radar and IMU is characterized in that: Step 2 includes: De-noising the IMU measurements; Integrate the measured values ​​after denoising; Perform error correction on the integration result; The error-corrected data is combined with the odometer measurement information to obtain the sensor data.

4. According to claim 1, the method for calculating the posture of an unmanned mining vehicle based on the fusion of laser radar and IMU is characterized in that: The steps in step three include: Obtaining motion scale information of the unmanned mining vehicle through the sensor data; According to the estimated posture and the motion scale information, continuous point cloud data are fused to obtain map information.

5. According to claim 1, the method for calculating the posture of an unmanned mining vehicle based on the fusion of laser radar and IMU is characterized in that: The steps to define the gyroscope bias estimation objective function include: The objective function of gyroscope bias estimation is: Among them, b ω Indicates the accelerometer gyroscope zero bias drift; δ indicates the error; and The external parameter rotation is obtained by combining the lidar and IMU through the lidar SFM; is the rotation pre-integrated estimate; is the pre-integrated rotation γ to the gyro zero bias b ω The Jacobian matrix of δb ω How does a small change in affect the value of γ? k represents the scanning frame number of the lidar, N represents the total number of scans of the lidar, γ represents the rotation pre-integration term of the IMU, and b k represents the zero bias drift of the accelerometer gyroscope in the body coordinate system in the kth frame, c0 represents the external parameter of the lidar in the odometer body coordinate system, represents the intersection, ‖‖ 2 It represents the norm, which is a mapping from a linear normed space to non-negative real numbers.

6. According to claim 1, the method for calculating the posture of an unmanned mining vehicle based on the fusion of laser radar and IMU is characterized in that: The optimized state vector in step 5 is: Among them, χ represents the optimized state vector, x k Represents n+1 frame point cloud x0,x1,…,x n , any one of λ m Represents the inverse depth of the m+1th feature point. The optimized state vector contains the n+1 IMU positions when sensing the 3D point cloud. attitude speed Gyro bias b g 、Accelerometer bias b a , LiDAR-IMU external parameter rotation and pan IMU-odometer extrinsic rotation and pan 7. The method for calculating the posture of an unmanned mining vehicle based on the fusion of laser radar and IMU according to claim 1 is characterized in that: The specific steps of step six include: Clustering point cloud data; Remove the ground from the clustered point cloud data to obtain the segmented object; For each segmented object, use deep learning methods to extract its features; Based on the KD tree algorithm, the newly collected point cloud data is matched with the already stored point cloud data; The consistency of the matching results is determined using the random sample consistency method. Only when the matching results pass the consistency verification is a closed loop determined.

8. The method for calculating the posture of an unmanned mining vehicle based on the fusion of laser radar and IMU according to claim 1 is characterized in that: The steps in step seven include: When a closed loop appears, a pose graph is created based on the optimized state vector. Each node in the pose graph represents a pose in an optimized state vector, and each edge represents a preset observation value. By solving the preset error function, all optimized state vectors are corrected to obtain the true pose of the unmanned mining car.

Images