Multi-sensor fusion laser radar SLAM system and method
By combining the tight coupling method of IMU and encoder in the lidar SLAM system, using slip detection and error state Kalman filters, the lidar SLAM drift problem in the underground environment is solved, and higher accuracy and robustness are achieved. Suitable for underground scenes such as mines.
Patent Information
- Application Number
- CN202310563144.7
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2023-05-18
- Publication Date
- 2024-11-22
- Estimated Expiration
- Not applicable · inactive patent
AI Technical Summary
In special environments such as long underground corridors, a single sensor is difficult to meet the accuracy and robustness requirements of lidar SLAM. Especially solid-state lidar has small FOV and few detected features, which are prone to drift problems. In addition, existing multi-sensor fusion is integrated. The solution has not been effectively applied to underground environments.
Using multi-sensor fusion lidar SLAM system, combined with the tight coupling method of IMU, encoder and solid-state lidar, through data preprocessing, slip detection and error state Kalman filter, the wheel encoder data is used to compensate IMU errors to form High precision and robust SLAM system.
It improves the accuracy and robustness of the SLAM system in the underground environment, is suitable for mines, flood drainage culverts and other scenarios, reduces lidar distortion and IMU error, and forms a stable local and global map.
Smart Images

Figure CN119001748A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the fields of SLAM, robots and electronic technologies, and in particular to a multi-sensor fusion laser radar SLAM system and method. Background Art
[0002] At present, the research on LiDAR SLAM technology has been very in-depth. However, for the special environment of long underground corridors, relying solely on a single sensor cannot meet the actual needs, because LiDAR relies on features to estimate posture, and long corridors have fewer features, so it is very easy to degrade and drift, especially for solid-state LiDAR, which has a relatively small FOV. Although its point cloud is denser, its FOV is only about 1 / 5 of that of mechanical LiDAR, so it can detect fewer features and is more prone to drift. In response to this, the SLAM method of multi-sensor fusion has begun to be promoted. High-precision positioning equipment such as fusion RTK / GPS / GNSS used in autonomous driving will fail in underground environments and are therefore not applicable.
[0003] IMU has become the solution for most underground environment SLAM. The defects of laser SLAM can be compensated by fusing IMU information, so that it is not affected by structural features and huge changes in the environment, and can provide high-precision attitude estimation in a short time. The solutions for the fusion of IMU and laser radar are mainly divided into loose coupling and tight coupling. The loosely coupled fusion solution can provide more accurate and stable work at the cost of minimal computation, that is, the IMU and laser radar data are processed independently, and once one of the sensors fails, the other sensor will still work. There are many solutions for loose coupling between mechanical laser radar and IMU, such as LOAM, LeGO-LOAM, cartographer, etc. Among the solutions for loose coupling between the emerging solid-state laser radar as the main sensor and IMU, LOAM-Livox draws on the LOAM algorithm and becomes the most representative solid-state laser radar SLAM algorithm. However, in current research, tight coupling is a research direction that researchers prefer, because with the continuous improvement of the performance of computing power equipment, the problem of computational complexity is gradually being overcome, so higher accuracy and robustness have become the pursuit.
[0004] The data of IMU and LiDAR will be calibrated and corrected with each other. The high-frequency data of IMU is used to offset the possible errors and drifts of LiDAR, so it is widely used in scenarios that require more sophisticated applications. In the scheme of using solid-state LiDAR and IMU tightly coupled, Fast-Lio is a proposed SLAM system based on iterative Kalman filter, which performs accurately and robustly in most scenarios, but the impact of its historical data on the current state will be discarded, and global posture correction cannot be performed. An iterative error state Kalman filter algorithm is also proposed in the prior art to tightly couple LiDAR / IMU, overcoming the accuracy loss caused by discarding high-order errors in the extended Kalman linearization process. However, its accuracy depends on the structured environment. As an improved version of Fast-Lio, Fast-Lio2 eliminates the feature extraction step and maintains the map through incremental kd-tree, which improves the robustness and efficiency of the algorithm and is suitable for most scenarios. It can be said to be the most advanced solution for the tight coupling of solid-state lidar and IMU. However, precisely because the algorithm has a higher universality, it does not take into account the application platform for special scenarios such as underground tunnels in mining areas and flood drainage culverts. That is, it may be used on wheeled robots, legged robots, or drones. Therefore, the actual environment and other sensors that may be used for fusion to improve the SLAM effect are not included. Summary of the invention
[0005] In order to solve the above technical problems, the present invention proposes a multi-sensor fusion lidar SLAM system and method, which not only gives play to the role of the wheel data of the wheeled robot itself, but also avoids the situation where the fusion of this data causes system instability by using slip detection means, so as to improve the accuracy and robustness of IMU / encoder / solid-state lidar tightly coupled SLAM according to local conditions and objects.
[0006] On the one hand, to achieve the above-mentioned purpose, the present invention provides a multi-sensor fusion laser radar SLAM system, comprising:
[0007] Data input module: used to obtain the collected raw data, which includes IMU data, wheel encoder data and point cloud data of solid-state laser radar;
[0008] Preprocessing module: used for preprocessing the raw data;
[0009] Pose estimation module: used for data fusion of IMU / encoder and laser radar, using the high-frequency data of the IMU / encoder to offset the distortion of the laser radar in a tightly coupled manner, and updating the IMU error state;
[0010] Output module: used to obtain the robot's odometer output and project the laser point cloud to the correct area through the odometer to form local maps and global maps;
[0011] Among them, the data input module, the preprocessing module, the posture estimation module, and the output module are connected in sequence.
[0012] Preferably, the data input module comprises:
[0013] IMU unit: used to collect the IMU data;
[0014] Wheel encoding unit: used to collect the wheel encoder data;
[0015] Solid-state LiDAR unit: used to collect point cloud data;
[0016] The IMU unit, the wheel encoding unit, and the solid-state laser radar unit are connected in sequence.
[0017] Preferably, the preprocessing module comprises:
[0018] IMU pre-integration unit: used to obtain the robot posture change predicted by the IMU data through pre-integration;
[0019] Wheel encoder mileage calculation unit: used to obtain the robot's linear speed and yaw angle;
[0020] Slip detection unit: used to determine whether the wheels of the robot are slipping, thereby improving the accuracy and robustness of the robot's posture prediction;
[0021] Among them, the IMU pre-integration unit is connected to the IMU unit, the wheel encoder mileage calculation unit is connected to the wheel encoder unit, and the slip detection unit is connected to the solid-state laser radar unit.
[0022] Preferably, the preprocessing module also includes a feature extraction and feature matching unit, which is used to extract features of the lidar point cloud data, extract plane points and edge points through the relationship between feature values, and use an ICP-based method to perform feature matching.
[0023] On the other hand, the present invention also provides an application method of a multi-sensor fusion laser radar SLAM system, comprising:
[0024] Acquire raw data based on a data input module, wherein the raw data includes IMU data, wheel encoder data, and point cloud data of a solid-state laser radar;
[0025] The raw data are respectively subjected to pre-integration processing, wheel encoder mileage calculation, slip detection, and feature extraction and matching by a pre-processing module to obtain processed raw data;
[0026] Through the pose estimation module, the error state Kalman filter is used to fuse the IMU / encoder and lidar data, and the high-frequency data of the IMU / encoder is used in a tightly coupled manner to offset the distortion of the lidar to obtain the updated IMU pose;
[0027] The robot posture is estimated after the ESKF filtering output, the robot's odometer output is obtained based on the output module, and the laser point cloud is projected to the correct area through the posture transformation of the filtered output to form a local map and a global map.
[0028] Preferably, the method for performing the pre-integration processing is:
[0029]
[0030]
[0031]
[0032] The superscript W in the upper left corner of the letter indicates the operation performed in the world coordinate system. is the rotation increment from the i-th to the j-th frame of the IMU, is the velocity variable of IMU from the i-th to the j-th frame, is the position variable of IMU from the i-th to the j-th frame, are the rotation increments at the i-th and j-th frames of the IMU, are the velocity increments at the i-th and j-th frames of the IMU, g is the gravitational acceleration, Δt is the measurement period, are the position increments at the i-th and j-th frames of the IMU respectively, and T is the transposed matrix.
[0033] Preferably, the method for calculating the wheel encoder mileage is:
[0034]
[0035] Among them, vl and vr are the linear speeds of the left and right front wheels respectively, are the rotation speeds of the left and right front wheels, r is the wheel radius, ψ is the yaw angle, is the linear velocity of the robot's xyz axes, and C is the distance between the robot's left and right wheels.
[0036] Preferably, performing the slip detection includes:
[0037] The angle change difference between the IMU and the wheel odometer in any time period is compared to determine whether the wheel is slipping. If it is slipping, the linear speed data of the IMU is used in this time period. If it is not slipping, the linear speed data is calculated using the wheel odometer.
[0038] Preferably, obtaining the updated IMU pose includes:
[0039] Define the IMU state and define the robot's position at the nth radar frame as The true state at the n+1th frame is defined as The error state is δx; then the state is propagated, and when a new IMU measurement frame arrives, the error state is updated to obtain the updated IMU pose.
[0040] Compared with the prior art, the present invention has the following advantages and technical effects:
[0041] Compared with only using the IMU and lidar coupling algorithm, the method of the present invention uses the error state Kalman filter to fuse the wheel encoder information containing slip detection, and compensates for the cumulative error of the IMU in linear speed, so that the proposed method has higher accuracy and robustness, and is suitable for underground scenes prone to degradation such as mine drainage culverts and tunnels. BRIEF DESCRIPTION OF THE DRAWINGS
[0042] The drawings constituting a part of the present application are used to provide a further understanding of the present application. The illustrative embodiments and descriptions of the present application are used to explain the present application and do not constitute an improper limitation on the present application. In the drawings:
[0043] Figure 1 A schematic diagram of a wheeled robot model and coordinate system definition in an embodiment of the present invention;
[0044] Figure 2 The present invention is a flowchart of an application method of a multi-sensor fusion laser radar SLAM system in an embodiment of the present invention. DETAILED DESCRIPTION
[0045] It should be noted that, in the absence of conflict, the embodiments and features in the embodiments of the present application can be combined with each other. The present application will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.
[0046] It should be noted that the steps shown in the flowcharts of the accompanying drawings can be executed in a computer system such as a set of computer executable instructions, and that, although a logical order is shown in the flowcharts, in some cases, the steps shown or described can be executed in an order different from that shown here.
[0047] The present invention proposes a multi-sensor fusion laser radar SLAM system, comprising:
[0048] Data input module: used to obtain the collected raw data, which includes IMU data, wheel encoder data and point cloud data of solid-state laser radar;
[0049] Preprocessing module: used for preprocessing the raw data;
[0050] Pose estimation module: used for data fusion of IMU / encoder and laser radar, using the high-frequency data of the IMU / encoder to offset the distortion of the laser radar in a tightly coupled manner, and updating the IMU error state;
[0051] Output module: used to obtain the robot's odometer output and project the laser point cloud to the correct area through the odometer to form local maps and global maps;
[0052] Among them, the data input module, the preprocessing module, the posture estimation module, and the output module are connected in sequence.
[0053] The physical picture and coordinate system diagram are as attached. Figure 1 The coordinate system is defined as follows: the main body coordinate system is B, the laser radar coordinate system is L, and the E l and E r It is defined as the coordinate system of the left front wheel and the right front wheel, and the IMU coordinate system is I. The world coordinate system is W, which is the initial center coordinate system of the robot.
[0054] The flowchart of the proposed method is shown in the attached Figure 2 As shown, specifically including:
[0055] (1) Input module: the raw data collected by the sensor, which includes IMU data, wheel encoder data, and solid-state lidar point cloud data, depending on the selected sensor.
[0056] (2) Preprocessing module: Preprocess the raw data of each sensor, including IMU pre-integration, wheel encoder mileage calculation, slip detection and feature extraction of lidar point cloud.
[0057] ①IMU pre-integration
[0058] The rotation, velocity and displacement increments in the pre-integration term from the i-th to the j-th radar frame are R, v and p respectively, and the measurement period is Δt. The pre-integration formula in the world coordinate system at the current moment can be expressed as follows:
[0059]
[0060]
[0061]
[0062] The superscript W in the upper left corner of the letter indicates that the operation is performed in the world coordinate system.
[0063] At this point, the robot posture changes predicted by IMU data can be obtained through pre-integration.
[0064] ② Wheel encoder mileage calculation
[0065] The radius of the wheels is r, and the rotation speeds of the left and right front wheels are defined as The time interval is recorded as Δt. The linear speed of the robot can be calculated by the encoder. and the yaw angle ψ, calculated as follows:
[0066]
[0067] Where C is the distance between the left and right wheels.
[0068] ③Slip detection
[0069] Since both the IMU and the wheel odometer can calculate linear velocity and angular velocity, the accuracy of the angular velocity calculated by the wheel odometer is much lower than that of the IMU, and the angular velocity will first diverge when the wheel slips. Therefore, the difference in angle changes between the two within a certain period of time is compared to determine whether the wheel is slipping. If it slips, the linear velocity data of the IMU is used during this period of time. If it does not slip, the linear velocity data calculated by the wheel odometer is used to improve the accuracy and robustness of the robot's posture prediction.
[0070] ④LiDAR point cloud feature extraction and feature matching
[0071] Due to the difference in scanning form between solid-state laser radar and mechanical laser radar, principal component analysis is used to extract point cloud features. The two feature points of plane points and edge points are extracted through the relationship between eigenvalues. Feature matching uses an ICP-based method to obtain the posture estimation obtained by the laser radar.
[0072] (3) Posture estimation module: The error state Kalman filter is used to fuse the IMU / encoder and lidar data, and the high-frequency data of the IMU / encoder is used to offset the distortion of the lidar in a tightly coupled manner. First, the state is defined, and the position of the robot at the nth radar frame (in the world coordinate system) is defined as The true state at the n+1th frame is defined as The error state is δx. Then the state propagation is performed. When a new IMU measurement frame arrives, the error state will be updated. The updated IMU error state can be expressed as follows:
[0073]
[0074] in, is the error state transfer matrix, J gt is the noise Jacobian matrix at time t, and w is the Gaussian noise.
[0075] The covariance matrix is calculated as follows:
[0076]
[0077] Where I is the identity matrix and Q is the covariance matrix of Gaussian noise.
[0078] When a new radar frame, i.e., the kth frame, arrives, the covariance of the forward propagation and the current pose are recorded as and
[0079] Finally, the status is updated using the following formula:
[0080]
[0081]
[0082]
[0083] in, and are generalized addition and subtraction, K is the Kalman gain, H is the measurement matrix, N is the noise covariance matrix, Represents the error between the measured value and the estimated value, I 15 is the 15th-order identity matrix.
[0084] (4) Output module
[0085] Through ESKF filtering, the robot's odometer output can be obtained, and the odometer is used to project the laser point cloud to the correct area to form a local map and a global map. At this point, a complete SLAM system is obtained.
[0086] The pose prediction of the IMU encoder and the pose estimate obtained by the LiDAR point cloud registration are input into the ESKF, which then outputs a more accurate pose estimate. ESKF is a filter that can correct the pose predicted by only a single sensor. The output of ESKF is the transformation of the robot pose from one frame to the next, which is a matrix. It is then multiplied with the existing laser point cloud to project it to the correct area. The point cloud map can be obtained through LiDAR scanning, but it is distorted. When ESKF outputs the odometer, the point cloud can be projected to the correct place to form a correct map.
[0087] The present invention solves the problems of degradation and drift of LiDAR SLAM in long underground corridors such as underground mine tunnels, flood drainage culverts, tunnels, etc. The robot slip detection is performed after preprocessing the wheel encoder data and IMU data, the encoder / IMU is adaptively selected to predict the robot posture, and the ESKF is tightly coupled with the solid-state LiDAR to form a high-precision and robust SLAM system.
[0088] Compared with the algorithm that only uses the coupling of IMU and lidar, the error state Kalman filter is used to fuse the wheel encoder information containing slip detection, and the cumulative error of IMU is compensated in linear speed, making the proposed method more accurate and robust, and suitable for underground scenes prone to degradation such as mine drainage culverts and tunnels.
[0089] The above are only preferred specific implementations of the present application, but the protection scope of the present application is not limited thereto. Any changes or substitutions that can be easily thought of by a person skilled in the art within the technical scope disclosed in the present application should be included in the protection scope of the present application. Therefore, the protection scope of the present application should be based on the protection scope of the claims.
Claims
1. A multi-sensor fusion laser radar SLAM system, characterized in that: include: Data input module: used to obtain the collected raw data, which includes IMU data, wheel encoder data and point cloud data of solid-state laser radar; Preprocessing module: used for preprocessing the raw data; Pose estimation module: used for data fusion of IMU / encoder and laser radar, using the high-frequency data of the IMU / encoder to offset the distortion of the laser radar in a tightly coupled manner, and updating the IMU error state; Output module: used to obtain the robot's odometer output and project the laser point cloud to the correct area through the odometer to form local maps and global maps; Among them, the data input module, the preprocessing module, the posture estimation module, and the output module are connected in sequence.
2. The multi-sensor fusion laser radar SLAM system according to claim 1, characterized in that: The data input module comprises: IMU unit: used to collect the IMU data; Wheel encoding unit: used to collect the wheel encoder data; Solid-state LiDAR unit: used to collect point cloud data; The IMU unit, the wheel encoding unit, and the solid-state laser radar unit are connected in sequence.
3. The multi-sensor fusion laser radar SLAM system according to claim 2 is characterized in that: The preprocessing module comprises: IMU pre-integration unit: used to obtain the robot posture change predicted by the IMU data through pre-integration; Wheel encoder mileage calculation unit: used to obtain the robot's linear speed and yaw angle; Slip detection unit: used to determine whether the wheels of the robot are slipping, thereby improving the accuracy and robustness of the robot's posture prediction; Among them, the IMU pre-integration unit is connected to the IMU unit, the wheel encoder mileage calculation unit is connected to the wheel encoder unit, and the slip detection unit is connected to the solid-state laser radar unit.
4. The multi-sensor fusion laser radar SLAM system according to claim 3 is characterized in that: The preprocessing module also includes a feature extraction and feature matching unit, which is used to extract features of the lidar point cloud data, extract plane points and edge points through the relationship between feature values, and use an ICP-based method to perform feature matching.
5. An application method of a multi-sensor fusion laser radar SLAM system, characterized in that: include: Acquire raw data based on a data input module, wherein the raw data includes IMU data, wheel encoder data, and point cloud data of a solid-state laser radar; The raw data are respectively subjected to pre-integration processing, wheel encoder mileage calculation, slip detection, and feature extraction and matching by a pre-processing module to obtain processed raw data; Through the pose estimation module, the error state Kalman filter is used to fuse the IMU / encoder and lidar data, and the high-frequency data of the IMU / encoder is used in a tightly coupled manner to offset the distortion of the lidar to obtain the updated IMU pose; The robot posture is estimated after the ESKF filtering output, the robot's odometer output is obtained based on the output module, and the laser point cloud is projected to the correct area through the posture transformation of the filtered output to form a local map and a global map.
6. The application method of the multi-sensor fusion laser radar SLAM system according to claim 5 is characterized in that: The method for performing the pre-integration processing is: The superscript W in the upper left corner of the letter indicates the operation performed in the world coordinate system. is the rotation increment from the i-th to the j-th frame of the IMU, is the velocity variable of IMU from the i-th to the j-th frame, is the position variable of IMU from the i-th to the j-th frame, are the rotation increments at the i-th and j-th frames of the IMU, are the velocity increments at the i-th and j-th frames of the IMU, g is the gravitational acceleration, Δt is the measurement period, are the position increments at the i-th and j-th frames of the IMU respectively, and T is the transposed matrix.
7. The application method of the multi-sensor fusion laser radar SLAM system according to claim 5, characterized in that: The method for calculating the wheel encoder mileage is: Among them, v l 、v r are the linear speeds of the left and right front wheels respectively, are the rotation speeds of the left and right front wheels, r is the wheel radius, ψ is the yaw angle, is the linear velocity of the robot's xyz axes, and C is the distance between the robot's left and right wheels.
8. The application method of the multi-sensor fusion laser radar SLAM system according to claim 5, characterized in that: The skid detection comprises: The angle change difference between the IMU and the wheel odometer in any time period is compared to determine whether the wheel is slipping. If it is slipping, the linear speed data of the IMU is used in this time period. If it is not slipping, the linear speed data is calculated using the wheel odometer.
9. The application method of the multi-sensor fusion laser radar SLAM system according to claim 5, characterized in that: Obtaining the updated IMU pose, including: Define the IMU state and define the robot's position at the nth radar frame as The true state at the n+1th frame is defined as The error state is δx; then the state is propagated, and when a new IMU measurement frame arrives, the error state is updated to obtain the updated IMU pose.
Citation Information
Patent Citations
Mapping method and system of tight coupling laser radar and inertial odometer
CN114526745A
Tight coupling positioning method and device based on ESKF and storage medium
CN114705210A
Robot simultaneous positioning and mapping method and device and readable medium
CN115265523A
Multi-sensor fusion SLAM (Simultaneous Localization and Mapping) method for monotonous texture-free large scene
CN115272596A
Three-dimensional reconstruction method and device, terminal equipment and computer readable medium
CN116091700A
Cited By
Robot pose determination method, robot and storage medium
CN119689496A
Laser radar point cloud acquisition method and system, electronic equipment and medium
CN122330906A
