Laser SLAM system and implementation method using IMU for slope compensation

Abstract

The present invention provides a laser SLAM system and implementation method for slope compensation using an IMU, the system comprising: a data preprocessing and slope detection module, for collecting and processing the original point cloud data inside the lane to obtain slope information; collecting and processing the IMU data of a mobile car to obtain the initial value of the car's posture and the IMU factor; a laser point cloud posture estimation module, for performing feature point classification and inter-frame matching posture estimation based on the slope information and the initial value of the car's posture, obtaining a joint error and a key frame factor; a high-precision mapping module, for performing factor graph optimization based on the joint error, the inter-frame matching posture estimation, the key frame factor and the IMU factor, and obtaining a global ramp map based on a local ramp map to complete slope compensation. The present invention has a wide range of applications and can be extended to complex environment SLAM on a variety of mobile platforms such as unmanned vehicles, drones, and underwater vehicles.

Description

Technical Field

[0001] The present invention belongs to the technical field of positioning and mapping of mobile robots, and in particular relates to a laser SLAM system and an implementation method for slope compensation using an IMU. Background Art

[0002] Nowadays, mobile robot SLAM technology has been widely studied. SLAM technology is not only widely used in the field of autonomous mobile robots and autonomous driving, but also plays an important role in special fields such as underground mines, tunnels, and coal mine tunnels. Complex environments such as irregular environments such as the lack of surface features of mine rock walls, dim environments without light, and bumpy and slippery roads often pose great challenges to navigation and motion control systems. In particular, when the ground is undulating, there are many ups and downs or multi-layer environments, the vehicle's posture often changes dramatically due to changes in road conditions. Traditional navigation systems are mostly based on plane assumptions, and it is difficult to accurately track and correct these posture changes, resulting in the accumulation of positioning errors and posture drift. At the same time, autonomous mobile robots cannot judge the passability of slopes, which not only affects the positioning and mapping accuracy of the vehicle, but may also affect the safety of navigation decisions.

[0003] The road surface inside some mine tunnels is complex, with large and uneven slopes. The movement of the robot will be affected by large pitch and bounce movements, resulting in large posture changes. Slope detection helps to build a more realistic map and improve positioning accuracy, which is of great significance for special scenes such as underground mines. Laser SLAM frameworks such as LOAM, Cartographer, and GMapping perform well under obvious structural features, especially indoor conditions, but the motion model assumed by the plane cannot correctly describe the actual movement under slope conditions. When going uphill, the point cloud reading is too large, the number of feature points decreases, and the kinematic assumption deviation increases; when going downhill, the point cloud generated in the occlusion area is dense and repeated, which makes feature matching difficult and the positioning drift increases. ZHANG J et al. proposed a lightweight, ground-optimized laser mileage calculation method LeGO-LOAM. It has high motion efficiency and extracts ground point references, but it is difficult to compensate for motion distortion under non-uniform speed, and the pose estimation is prone to divergence in large scenes. Carpenter et al. proposed a particle filter (PF) by extending the Kalman filter. This method is based on the Bayesian filtering framework and uses Monte Carlo importance sampling to adapt to the nonlinear problems existing in SLAM. Compared with Kalman filtering, which only relies on the linear Gaussian assumption, particle filtering has the ability to handle nonlinear and non-Gaussian noise, and can effectively handle the nonlinear problems caused by robot motion and sensor measurement. Tixiao Shan et al. proposed LIO-SAM based on the optimization of laser and inertial graphs, and then added visual information to propose LVI-SAM. Jia et al. proposed a tightly coupled multi-sensor fusion framework Lvio-Fusion. Most of the research uses IMU as a data source and fuses it with other sensors to improve the reliability of state estimation, ignoring the active analysis of the slope information provided by IMU to correct the motion drift generated in the slope environment. Therefore, it is necessary to establish a more reasonable nonlinear model, increase the constraints of positioning, effectively analyze and utilize IMU information, and realize active compensation through algorithmic means to make the SLAM system adapt to complex environments. Summary of the invention

[0004] In view of the shortcomings of the prior art, the present invention proposes a laser SLAM system and implementation method for slope compensation using IMU, constructs a new observation model based on IMU data, and integrates posture data into the graph optimization framework for constraint, thereby realizing active compensation of the SLAM algorithm. This method can not only improve the accuracy of positioning and mapping, but also enable the robot to judge the passability of the terrain based on this, avoiding dangerous decisions in complex environments. In general, the solution combines point cloud processing with graph optimization, and uses active analysis and correction of IMU information to enable the SLAM system to adapt to various complex environment terrains.

[0005] To achieve the above object, the present invention provides the following solutions:

[0006] Laser SLAM system using IMU for slope compensation, including:

[0007] The data preprocessing and slope detection module is used to collect and process the original point cloud data inside the tunnel to obtain slope information; collect and process the IMU data of the mobile car to obtain the initial value of the car's posture and IMU factor;

[0008] The laser point cloud pose estimation module is used to classify feature points based on the slope information and the initial value of the vehicle pose to obtain joint errors; perform inter-frame matching pose estimation and key frame factors;

[0009] The high-precision mapping module is used to optimize the factor graph based on the joint error, the inter-frame matching pose estimation, the key frame factor and the IMU factor, and obtain the global map of the ramp based on the local map of the ramp to complete the slope compensation.

[0010] Preferably, the data preprocessing and slope detection module includes:

[0011] A point cloud data processing unit is used to collect the original point cloud data inside the lane, and filter the original point cloud data to obtain the ramp point cloud information; a random sampling consensus algorithm is used to segment and fit the ramp point cloud data to obtain the slope information;

[0012] The IMU data processing unit is used to collect IMU data of the mobile car, and perform IMU pre-integration based on the ramp point cloud information to obtain the initial value of the car's posture.

[0013] Preferably, the point cloud data processing unit comprises:

[0014] Point cloud data acquisition subunit, used to collect original point cloud data inside the tunnel;

[0015] The filtering subunit is used to filter out outliers in the original point cloud data inside the lane based on a statistical filter to obtain a point cloud containing only the four walls of the lane and the ground; to filter out the four wall point clouds of the lane from the point clouds containing the four walls of the lane and the ground based on a straight-through filter to obtain a slope point cloud with a preset contour; to filter the slope point cloud based on a dynamic voxel filter to obtain the ramp point cloud information; wherein the ramp point cloud information includes the ground point cloud and the slope point cloud;

[0016] The slope detection subunit is used to segment and fit the slope point cloud in the ramp point cloud information based on the RANSAC random sampling consistency algorithm to obtain the slope information.

[0017] Preferably, the specific process of the slope detection subunit obtaining the slope information is:

[0018] Obtaining a set of parameters to be estimated based on a preset loss function, a threshold constant, and an error function of a set distance;

[0019] Fitting the parameters to be estimated to obtain optimal parameters and constructing the slope point cloud fitting model;

[0020] The slope information is obtained based on the slope point cloud fitting model.

[0021] Preferably, the IMU data processing unit includes:

[0022] The IMU data acquisition subunit is used to obtain the pitch angle and roll angle of the mobile car in the navigation coordinate system based on the three-axis gyroscope and three-axis accelerometer fixed on the mobile car;

[0023] An IMU data processing subunit is used to convert the pitch angle and the roll angle into a direction cosine matrix and establish an IMU pre-integration observation model;

[0024] The distortion correction subunit is used to perform distortion correction on the original point cloud data inside the lane based on the IMU pre-integration observation model, and use local smoothness evaluation variables to divide corner points and surface points to obtain the initial value of the vehicle posture.

[0025] Preferably, the laser point cloud pose estimation module includes:

[0026] The feature point classification unit is used to compare the point clouds before and after the straight-through filtering to obtain the slope point cloud, and classify the feature points based on the angle information of the pitch angle and the roll angle;

[0027] A joint error construction subunit is used for adding a slope constraint to the mobile car based on joint graph optimization of feature points and subgraphs to obtain the joint error;

[0028] The inter-frame matching posture estimation subunit is used to perform inter-frame matching posture estimation based on the slope information and the initial value of the vehicle posture to obtain the key frame factor.

[0029] Preferably, the feature points include ground points, slope points and wall points.

[0030] The present invention also provides a method for implementing a laser SLAM system using an IMU for slope compensation, comprising the following steps:

[0031] Collect and process the original point cloud data inside the tunnel to obtain slope information; collect and process the IMU data of the mobile car to obtain the initial value of the car's posture and IMU factor;

[0032] Based on the slope information and the initial value of the vehicle posture, feature point classification is performed to obtain joint error; inter-frame matching posture estimation and key frame factor are performed;

[0033] Based on the joint error, the inter-frame matching pose estimation, the key frame factor and the IMU factor, factor graph optimization is performed, and based on the ramp local map, a ramp global map is obtained to complete slope compensation.

[0034] Compared with the prior art, the beneficial effects of the present invention are as follows: the laser SLAM system based on IMU slope compensation proposed in the present invention can significantly improve the positioning and mapping accuracy of mobile robots in complex environments. According to the point cloud data collected inside the lane, the slope model can be fitted and the slope detection can be performed, providing prior information for posture estimation. Compared with the traditional technology of simply fusing IMU as a sub-sensor, this method can actively analyze IMU information, and after adding multi-factor graph optimization, effective posture compensation can be performed, which enhances the adaptability to complex terrain. The specific effects are shown in the attached figure. Figure 4 As shown in the figure, the positioning error of the mobile robot can be reduced by 20% under the consideration of slope constraints. At the same time, the error rate of point cloud registration can be reduced, and the convergence speed of filters such as EKF can be improved, which is of great help to the passage decision of small robots, so that they can build more complete and detailed maps. This technology has a wide range of applications and can be extended to complex environment SLAM on various mobile platforms such as unmanned vehicles, drones, and underwater vehicles. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] In order to more clearly illustrate the technical solution of the present invention, the following briefly introduces the drawings required for use in the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.

[0036] Figure 1 It is a structural schematic diagram of a laser SLAM system using an IMU for slope compensation in an embodiment of the present invention;

[0037] Figure 2 This is an internal framework diagram of a laser SLAM system that uses an IMU for slope compensation in an embodiment of the present invention;

[0038] Figure 3 This is a flow chart of slope recognition based on Lidar point cloud in an embodiment of the present invention;

[0039] Figure 4 This is a schematic diagram of multi-factor joint graph optimization with slope constraints in an embodiment of the present invention;

[0040] Figure 5 This is a diagram of the expected optimization effect in an embodiment of the present invention. DETAILED DESCRIPTION

[0041] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0042] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

[0043] Embodiment 1

[0044] In view of the fact that the large terrain fluctuations in underground mining environments cause drastic changes in the robot's posture, the positioning and mapping accuracy of the SLAM algorithm based on the plane assumption is seriously affected. The existing technology only uses IMU as a sub-sensor data source for simple fusion, and cannot achieve active compensation for such posture changes. In addition, in dim underground tunnels, the robot cannot accurately judge the ground slope information, and navigation decisions may face safety hazards. The present invention proposes a laser SLAM system that uses IMU for slope compensation for the interior of complex tunnels with large fluctuations, so that compensation can be performed based on slope data, such as Figure 1 , Figure 2 As shown, it includes: data preprocessing and slope detection module, laser point cloud pose estimation module (front-end laser radar odometer) and high-precision mapping module (back-end factor graph optimization).

[0045] The mobile robot (mobile car) is equipped with multi-line laser radar and IMU and other equipment, and the final global map is output through point cloud data processing, front-end laser radar odometer, and back-end factor graph optimization. Since the IMU directly outputs the pitch angle data of the mobile robot at the instant, rather than the angle data of the ramp, if the ramp angle information can be added, the ground points, slope points and lane wall points can be separated according to the angle information. The present invention adds a slope detection unit after point cloud filtering, which can provide prior information for the robot's posture transformation and reduce the error caused by feature matching. It can also add corresponding slope constraints in the laser odometer, and output the global map through multi-factor graph optimization. According to the climbing ability of different mobile cars, the ramps that the car cannot pass through are marked in the corresponding map, and the ramps with less impact are marked as flat ground in the three-dimensional map, providing necessary information for the robot's decision-making and movement.

[0046] Specifically, the data preprocessing and slope detection module is used to collect and process the original point cloud data inside the tunnel to obtain slope information; collect and process the IMU data of the mobile vehicle to obtain the initial value of the vehicle's posture and IMU factor; among them, the IMU factor refers to the data obtained by the IMU (inertial measurement unit), such as tilt and rotation information. As a constraint, it is added to the factor graph optimization of the back end and combined with the lidar data to improve positioning and mapping accuracy.

[0047] A further implementation method is that the data preprocessing and slope detection module includes:

[0048] The point cloud data processing unit is used to collect the original point cloud data inside the lane, filter the original point cloud data, and obtain the ramp point cloud information; the random sampling consensus algorithm is used to segment and fit the ramp point cloud data to obtain the slope information;

[0049] The IMU data processing unit is used to collect the IMU data of the mobile car and perform IMU pre-integration based on the ramp point cloud information to obtain the initial value of the car's posture.

[0050] like Figure 3 As shown, a further implementation is that the point cloud data processing unit includes:

[0051] Point cloud data acquisition subunit, used to collect original point cloud data inside the tunnel;

[0052] The filtering subunit is used to filter out outliers in the original point cloud data inside the tunnel based on a statistical filter to obtain a point cloud containing only the four walls of the tunnel and the ground; to filter out the four wall point clouds of the tunnel from the point clouds containing the four walls of the tunnel and the ground based on a straight-through filter to obtain a slope point cloud with a preset contour; to filter the slope point cloud based on a dynamic voxel filter to obtain ramp point cloud information; wherein the ramp point cloud information includes the ground point cloud and the slope point cloud;

[0053] In this embodiment, for the original point cloud data P inside the lane i There is some noise, so we can consider using a statistical filter to filter out outliers and obtain a point cloud P containing only the four walls of the tunnel and the ground. s , because the slope needs to be identified, it is necessary to eliminate the interference of the tunnel wall point cloud. The straight-through filtering method is used to filter the target slope point cloud x, y, z axes to filter the slope point cloud with the specified contour [x1, y1, z1]. At this time, P s It contains a large amount of point cloud data. It is difficult to fit directly and prone to errors. We choose to use dynamic voxel filtering to process the slope point cloud. The surface change is judged by the consistency of the normal vector of the point cloud. The voxels are reduced in places where the change is drastic, and the voxel boundary points are retained to avoid oversampling and information loss. The point cloud filtering is as follows:

[0054]

[0055] Among them, P g represents the point cloud after voxel filtering, P j Represents the source point cloud, Vi represents the i-th voxel grid, ni is the number of point clouds contained in this voxel grid, and the center of gravity of each voxel is used to represent all the point clouds of the grid. On this basis, dynamic adjustment is performed. The dynamic adjustment process is shown in Formula 2:

[0056]

[0057] Among them, np is the normal vector of each point, ΔSi is the consistency representation of the normal vector, is the average direction of the point normal vector inside the voxel Si, Represents the deviation between a single point and the average direction. If ΔSi exceeds the consistency threshold condition τ1, the voxel is linearly reduced by a ratio r (usually 0.5). When Si' is less than the minimum voxel side length, stop and record the boundary point set Bnd of the original voxel Si. Si , keep Bnd when generating Si' Si The internal points finally output the filtered ramp point cloud P rmp .

[0058] The slope detection subunit is used to segment and fit the slope point cloud in the ramp point cloud information based on the RANSAC random sampling consistency algorithm to obtain the slope information.

[0059] A further implementation method is that the specific process of the slope detection subunit obtaining the slope information is as follows:

[0060] Obtaining a set of parameters to be estimated based on a preset loss function, a threshold constant, and an error function of a set distance;

[0061] Fit the estimated parameters to obtain the optimal parameters and build a slope point cloud fitting model;

[0062] The slope information is obtained based on the slope point cloud fitting model.

[0063] In this embodiment, the collected Lidar ramp point cloud information is mainly composed of the ground and the slope surface. The RANSAC random sampling consistency algorithm can be used to segment and fit the slope point cloud. Assume that the slope point cloud fitting model is expressed as Formula 3:

[0064] α0x+β0y+χ0z+δ0=0 (3)

[0065] Among them, α, β, χ, and δ are the parameters to be estimated. The initial value of the slope model is estimated first, and then the optimal parameters of the model are obtained by fitting. rmpm=[α, β, χ, δ], the parameter set is obtained as formula 4:

[0066]

[0067] Where ramp is the Lidar point cloud, Loss is the loss function of the computational model, T is the threshold constant, σrp is the loss function constant, and Err is the error function of the set distance. If the ground point cloud simulation fitting is expressed as α1x+β1y+χ1z+δ1=0, then the angle of the ramp, i.e. the slope, can be expressed as Equation 5:

[0068]

[0069] where θ κ is the error correction variable, θ rmp This is the slope value to be measured. Now you can get the overall slope information data of the required ramp area.

[0070] A further embodiment is that the IMU data processing unit comprises:

[0071] The IMU data acquisition subunit is used to obtain the pitch angle and roll angle of the mobile car in the navigation coordinate system based on the three-axis gyroscope and three-axis accelerometer fixed on the mobile car;

[0072] The IMU data processing subunit is used to convert the pitch angle and roll angle into a direction cosine matrix and establish an IMU pre-integration observation model;

[0073] The distortion correction subunit is used to perform distortion correction on the original point cloud data inside the lane based on the IMU pre-integration observation model, and use the local smoothness evaluation variable to divide the corner points and surface points to obtain the initial value of the vehicle's posture.

[0074] In this embodiment, an IMU containing a three-axis gyroscope and a three-axis accelerometer is fixed on the mobile vehicle, and the accelerometer is used to detect the direction of gravity in real time to calculate the pitch angle θ of the mobile platform in the navigation coordinate system w. pitch and roll angle θ roll , change the pitch angle θ pitch and roll angle θ roll Convert to direction cosine matrix Represents the rotation relationship from the IMU coordinate system to the navigation coordinate system. Since the point cloud p after voxel filtering g It only contains the point cloud information of the ramp. To perform SLAM mapping of the entire lane, it is necessary to use the point cloud data P s Secondary processing is performed, and the smoothness evaluation variable c is used to divide corner points and surface points. The calculation method of the local plane local smoothness c is formula 6:

[0075]

[0076] Where s is the number of points in the adjacent point domain, is the coordinate of point i at time k in the Lidar coordinate system, is the coordinate of point j at the same moment.

[0077] The laser point cloud pose estimation module is used to classify feature points based on the slope information and the initial value of the vehicle pose to obtain joint errors; perform inter-frame matching pose estimation and key frame factors;

[0078] The high-precision mapping module is used to optimize the factor graph based on the joint error, inter-frame matching pose estimation, key frame factor and IMU factor, and obtain the global map of the ramp based on the local map of the ramp to complete the slope compensation.

[0079] A further implementation is that the laser point cloud pose estimation module includes:

[0080] The feature point classification unit is used to compare the point clouds before and after the straight-through filtering to obtain the slope point cloud, and classify the feature points in combination with the angle information of the pitch angle and the roll angle; a further implementation method is that the feature points include ground points, slope points and wall points.

[0081] The joint error construction subunit is used to optimize the joint graph based on feature points and subgraphs, add slope constraints to the mobile car, and obtain the joint error;

[0082] like Figure 4 As shown, in this embodiment, the slope point cloud is obtained by comparing the point cloud before and after the straight-through filtering, and the feature points are divided into ground points F according to the angle information. d 、Slope point F p and wall point F q , separate the characteristic slope points to reduce the error of point cloud matching caused by ups and downslopes, that is, use the surface feature roll angle, pitch angle and line feature heading angle of the characteristic points for hierarchical matching. When the mobile robot is going up and down slopes, a slope constraint can be added to the z-axis displacement of the key frame to reduce the positioning error. Due to the complex conditions of the roadway ramp, it is not possible to add constraints to the posture graph constructed by normal plane driving. Consider using subgraph-based joint graph optimization. That is, through the IMU slope constraint and the registration factor, the z-axis displacement of the key frame is used to reduce the positioning error. According to the fitted slope parameter set P rmpm , convert the slope parameters at the current moment into the Lidar coordinate system. The transformation process is shown in equations 7 and 8:

[0083] [n' α ,n' β ,n' χ ] T =R t [n α,n β ,n χ ] T (7)

[0084] d'=dt t [n' α ,n' β ,n' χ ] T (8)

[0085] where n α ,n β ,n χ is the ramp plane parameter, R t is the rotation of the laser radar at time t. t is the translation of the laser radar at time t, and d' represents the distance from the slope to the laser radar after translation. At this time, the error between the attitude node and the ramp node is e pose , and the angle information of the IMU is constructed into an error term and added to the joint graph optimization, expressed as equations 9, 10, and 11:

[0086]

[0087]

[0088] Δ=e imu ^T·∑imu ^-1 ·e pose (11)

[0089] in is the slope coordinate in the laser radar coordinate system with added angle information, Δ is the Mahalanobis distance of the error term, ∑imu^ -1 is the inverse of the covariance matrix of the error term, is the optimization variable to be estimated after optimization.

[0090] The inter-frame matching pose estimation subunit is used to perform inter-frame matching pose estimation based on the slope information and the initial value of the vehicle pose to obtain the key frame factor.

[0091] In this embodiment,

[0092] Assume that the Lidar key frame i is from t i At the beginning of time, the scan point cloud is P s , the data collected by IMU in a short period of time is I (i,j) , the system is at t i The Lie group of the rotation matrix at time is R i , the speed is v i , IMU zero bias is b i , x iis the state of the key frame. IMU pre-integration can transform high-frequency raw observations into constraints between state quantities, providing important prior information for back-end graph optimization. The IMU pre-integration observation model is equations 12, 13, and 14:

[0093]

[0094]

[0095]

[0096] in, is the rotation, speed, and displacement corresponding to the ijth key frame in the time interval t, R i and R j represents the rotation matrix of the corresponding frame, v j and v i Indicates the speed of the corresponding frame, s j and i Represents the displacement of the corresponding frame, the tail term is a random variable, and the error e of the attitude node and the environment node of the joint IMU pose , δ refers to the deviation of the corresponding measurement time integral, and φ refers to the exponential mapping of the attitude rotation matrix. Add the minimum constraint of the joint error term as shown in equation 15:

[0097]

[0098] They are the errors between the rotation vector, velocity vector, displacement vector and the actual posture transformation. Figure 5 As shown in the figure, the laser odometer pose estimation is finally optimized by factor graph, and the key frame matching map is updated in a san-to-map manner. Specifically, the local map of the ramp is updated, and the factor graph is optimized based on the key frame factor, IMU factor, minimum constraint of joint error term and map registration. The local map factor optimized by the factor graph and the inter-frame matching pose estimation are then registered. Finally, the global map is output based on the map registration and the local map update of the ramp.

[0099] Embodiment 2

[0100] The present invention also provides a method for implementing a laser SLAM system using an IMU for slope compensation, comprising the following steps:

[0101] Collect and process the original point cloud data inside the tunnel to obtain slope information; collect and process the IMU data of the mobile car to obtain the initial value of the car's posture and IMU factor;

[0102] Based on the slope information and the initial value of the vehicle posture, feature point classification is performed to obtain a joint error; inter-frame matching posture estimation and key frame factors are performed;

[0103] Based on the joint error, inter-frame matching pose estimation, key frame factor and IMU factor, factor graph optimization is performed, and based on the local map of the ramp, the global map of the ramp is obtained to complete the slope compensation.

[0104] The embodiments described above are only descriptions of the preferred embodiments of the present invention and are not intended to limit the scope of the present invention. Without departing from the design spirit of the present invention, various modifications and improvements made to the technical solutions of the present invention by ordinary technicians in this field should all fall within the protection scope determined by the claims of the present invention.

Claims

1. A laser SLAM system using IMU for slope compensation, characterized in that: include: Data preprocessing and slope detection module, used to collect and process the original point cloud data inside the tunnel to obtain slope information; Collect and process the IMU data of the mobile car to obtain the initial value of the car's posture and IMU factor; The data preprocessing and slope detection module includes: A point cloud data processing unit is used to collect the original point cloud data inside the lane, and filter the original point cloud data to obtain the ramp point cloud information; a random sampling consensus algorithm is used to segment and fit the ramp point cloud data to obtain the slope information; An IMU data processing unit is used to collect IMU data of the mobile car, and perform IMU pre-integration based on the ramp point cloud information to obtain the initial value of the car's posture; The point cloud data processing unit comprises: Point cloud data acquisition subunit, used to collect original point cloud data inside the tunnel; The filtering subunit is used to filter out outliers in the original point cloud data inside the lane based on a statistical filter to obtain a point cloud containing only the four walls of the lane and the ground; to filter out the four wall point clouds of the lane from the point clouds containing the four walls of the lane and the ground based on a straight-through filter to obtain a slope point cloud with a preset contour; to filter the slope point cloud based on a dynamic voxel filter to obtain the ramp point cloud information; wherein the ramp point cloud information includes the ground point cloud and the slope point cloud; A slope detection subunit is used to segment and fit the slope point cloud in the ramp point cloud information based on the RANSAC random sampling consistency algorithm to obtain the slope information; The specific process of the slope detection subunit obtaining the slope information is as follows: Obtaining a set of parameters to be estimated based on a preset loss function, a threshold constant, and an error function of a set distance; Fitting the parameters to be estimated to obtain optimal parameters and constructing the slope point cloud fitting model; Based on the slope point cloud fitting model, obtaining the slope information; The collected Lidar ramp point cloud information consists of the ground and the slope surface. The RANSAC random sampling consistency algorithm is used to segment and fit the slope point cloud. It is assumed that the slope point cloud fitting model is expressed as: α0x+β0y+χ0z+δ0=0 Among them, α, β, χ, and δ are the parameters to be estimated. The initial value of the slope model is estimated first, and then the optimal parameters of the model are obtained by fitting. rmpm =[α, β, χ, δ], the parameter set is obtained as follows: Among them, ramp is the Lidar point cloud, Loss is the loss function of the calculation model, T is the threshold constant, σrp is the loss function constant, and Err is the error function of the set distance; if the ground point cloud simulation fitting is expressed as α1x+β1y+χ1z+δ1=0, then the angle of the ramp, that is, the slope, is expressed as: where θ κ is the error correction variable, θ rmp This is the slope value to be measured, and the overall slope information data of the required ramp area is obtained; The IMU data processing unit includes: The IMU data acquisition subunit is used to obtain the pitch angle and roll angle of the mobile car in the navigation coordinate system based on the three-axis gyroscope and three-axis accelerometer fixed on the mobile car; An IMU data processing subunit is used to convert the pitch angle and the roll angle into a direction cosine matrix and establish an IMU pre-integration observation model; A distortion correction subunit, used to perform distortion correction on the original point cloud data inside the lane based on the IMU pre-integration observation model, and divide the corner points and surface points using a local smoothness evaluation variable to obtain an initial value of the vehicle posture; The IMU containing a three-axis gyroscope and a three-axis accelerometer is fixed on the mobile car. The accelerometer detects the direction of gravity in real time and calculates the pitch angle θ of the mobile platform in the navigation coordinate system w. pitch and roll angle θ roll , the pitch angle θ pitch and roll angle θ roll Convert to direction cosine matrix Represents the rotation relationship from the IMU coordinate system to the navigation coordinate system; since the point cloud p after voxel filtering g It only contains the point cloud information of the ramp. To perform SLAM mapping of the entire lane, it is necessary to use the point cloud data P s Secondary processing is performed, and the smoothness evaluation variable c is used to divide corner points and surface points. The calculation method of the local plane local smoothness c is: Where s is the number of points in the adjacent point domain, is the coordinate of point i at time k in the Lidar coordinate system, is the coordinate of point j at the same moment; a laser point cloud pose estimation module is used to classify feature points based on the slope information and the initial value of the vehicle pose to obtain a joint error; perform inter-frame matching pose estimation to obtain a key frame factor; The laser point cloud pose estimation module comprises: A feature point classification unit is used to compare the point clouds before and after the straight-through filtering to obtain the slope point cloud, and classify the feature points in combination with the angle information of the pitch angle and the roll angle; the feature points include ground points, slope points and wall points; A joint error construction subunit is used for adding a slope constraint to the mobile car based on joint graph optimization of feature points and subgraphs to obtain the joint error; The slope point cloud is obtained by comparing the point cloud before and after the straight-through filtering, and the feature points are divided into ground points F according to the angle information. d 、Slope point F p and wall point F q , separate the characteristic slope points to reduce the error of point cloud matching caused by ups and downslopes, that is, use the surface feature roll angle, pitch angle and line feature heading angle of the characteristic points for hierarchical matching; when the mobile robot is going up and down slopes, the IMU slope constraint and registration factor are used to reduce the positioning error as the z-axis displacement of the key frame; according to the fitted slope parameter set P rmpm , convert the slope parameters at the current moment into the Lidar coordinate system. The transformation process is as follows: [n' α ,n' β ,n' χ ] T =R t [n α ,n β ,n χ ] T d'=d-t t [n' α ,n' β ,n' χ ] T where n α ,n β ,n χ is the ramp plane parameter, R t is the rotation of the laser radar at time t; t t is the translation of the laser radar at time t, d' represents the distance from the slope to the laser radar after translation; at this time, the error between the attitude node and the ramp node is e pose , and the angle information of the IMU is constructed into an error term and added to the joint graph optimization, which is expressed as: Δ=e imu ^T·Simu^ -1 ·e pose in is the slope coordinate in the laser radar coordinate system with added angle information, Δ is the Mahalanobis distance of the error term, ∑imu^ -1 is the inverse of the covariance matrix of the error term, is the optimization variable to be estimated after optimization; An inter-frame matching posture estimation subunit is used to perform inter-frame matching posture estimation based on the slope information and the initial value of the vehicle posture to obtain the key frame factor; The high-precision mapping module is used to optimize the factor graph based on the joint error, the inter-frame matching pose estimation, the key frame factor and the IMU factor, and obtain the global map of the ramp based on the local map of the ramp to complete the slope compensation.

2. A method for implementing a laser SLAM system using an IMU for slope compensation, which is used to implement the laser SLAM system using an IMU for slope compensation as claimed in claim 1, characterized in that: The following steps are involved: Collect and process the original point cloud data inside the tunnel to obtain slope information; collect and process the IMU data of the mobile car to obtain the initial value of the car's posture and IMU factor; Based on the slope information and the initial value of the vehicle posture, feature point classification is performed to obtain a joint error; inter-frame matching posture estimation is performed to obtain a key frame factor; Based on the joint error, the inter-frame matching pose estimation, the key frame factor and the IMU factor, factor graph optimization is performed, and based on the ramp local map, a ramp global map is obtained to complete slope compensation.

Images