Multi-story building mapping method and system based on laser radar and IMU coupling and medium

Abstract

The invention discloses a multi-story building mapping method and system based on laser radar and IMU coupling and a medium, and the method comprises the steps: scanning and collecting environment information through a laser radar sensor to obtain point cloud data, carrying out the motion distortion removal of a laser radar point cloud through IMU input data, and carrying out the down-sampling of the point cloud data through a grid down-sampling algorithm; carrying out ground point cloud removal on the single-frame point cloud based on a Markov random field; constructing an optimized pose map based on a multi-plane ground constraint pose constraint algorithm; calculating to obtain a fusion error value by using an error fusion algorithm based on Kalman filtering, and compensating motion estimation; registering and fusing the point cloud data of the current key frame and the global map obtained by accumulation at the preorder moment through a point iteration ICP algorithm; according to the mapping method, the phenomena of mapping drifting and scene degradation are effectively restrained for corridor scenes, upstairs and downstairs and use scenes of multiple similar floors with similar indoor mapping.

Description

Technical Field

[0001] The present invention relates to the field of map construction technology, and in particular to a multi-story building mapping method, system and medium based on laser radar and IMU coupling. Background Art

[0002] LiDAR uses laser beams to measure distance and perceive the environment to complete three-dimensional modeling. It is mainly used in the measurement and modeling of buildings and infrastructure, mapping and analysis of terrain and landforms, and object detection and recognition.

[0003] In the process of modeling indoor building structures, laser radar is used to scan maps to obtain three-dimensional point cloud data of buildings, describing the shape, size, structure and other detailed information of the buildings; by processing these data, a three-dimensional model of the building structure is generated. In practical applications, laser radar and IMU inertial sensor coupling mapping are often used to complete the construction of three-dimensional models. Although the spatial position information of the building can be accurately described during the modeling process, it is difficult to obtain features such as texture and color on the surface of the building structure.

[0004] In indoor multi-story building structures, there are similar narrow and unstructured spaces, such as corridors and stairs with similar texture features, multi-plane steps, etc. Therefore, a mapping method applied to indoor building structures is needed to improve measurement accuracy and reliability. Summary of the invention

[0005] The present application provides a multi-story building mapping method, system and medium based on laser radar and IMU coupling, thereby solving the problem of posture drift in the prior art during the modeling of multi-story indoor building structures due to the existence of similar structures and narrow structures, and achieving high-quality modeling of multi-story building structures.

[0006] The present application provides a multi-story building mapping method based on laser radar and IMU coupling, comprising the following steps:

[0007] Step S1, the laser radar sensor scans and collects environmental information to obtain point cloud data, uses IMU input data to remove motion distortion of the laser radar point cloud, and uses a grid downsampling algorithm to downsample the point cloud data;

[0008] Step S2, removing ground point clouds from single-frame point clouds based on Markov random fields, highlighting key features of point cloud data, and accumulating key frame data for the processed single-frame point clouds; removing ground point clouds from single-frame point clouds includes expanding each point cloud ring, completing obstacle recognition based on the height difference between two points in the same ring, using the binary classification task of the graph to distinguish the ground point clouds, and completing ground point recognition by constraining the segmentation quality through the Markov random field;

[0009] Step S3, constructing an optimized pose graph based on a multi-plane ground constraint pose constraint algorithm; the constraint algorithm includes: uniformly projecting the ground point cloud into the current sensor coordinate system, fitting a regression equation to all projection points, judging three scenes of going upstairs, going downstairs, and flat ground; constructing constraints for the three scenes of going upstairs, going downstairs, and flat ground; adding the constraints as edges in the pose graph;

[0010] Step S4, according to the back propagation module after the key frame data accumulation in step S2, the historical cumulative error value obtained by the current map is calculated; the pose error values ​​of different pose sources are obtained by combining the pose graph obtained by the constraint algorithm in step S3; the fusion error value is calculated by using the error fusion algorithm based on Kalman filtering, and the motion estimation is compensated;

[0011] Step S5: In step S5, the point cloud data of the current key frame is registered and fused with the accumulated global map of the previous moment through the point iteration ICP algorithm, and the newly added point cloud data is registered into the dynamic kd tree to speed up the subsequent point cloud registration step.

[0012] Preferably, in step S1, the point cloud data is downsampled using a grid downsampling algorithm, specifically:

[0013] For point p i =(x i ,y i ,z i ), with a grid size of v x ,v y ,v z When the grid index Voxel(p i )=(i,j,k), where:

[0014]

[0015] For all points in the grid, form a point set:

[0016]

[0017] After calculating all the points in the point set, the centroid of all points is used to represent the point cloud data of the grid, which is specifically expressed as:

[0018]

[0019] Preferably, the obstacle identification in step S2 is specifically as follows:

[0020] When the height difference Δd between two points in the same circle is less than the threshold, there is an obstacle between the two points:

[0021]

[0022] Where h is the installation height of the laser radar, δ 1 and δ 2 is the angle between the laser beam and the ground normal, and K is the maximum height difference and elevation angle that can be set.

[0023] Preferably, the ground point identification in step S2 is specifically as follows:

[0024] The energy function E(A) = λ·R(A) + B(A) is used to constrain the segmentation results, where R(A) is defined as the error penalty for node p being assigned to the background or object, and B(A) is defined as the penalty coefficient B defined as the boundary discontinuity between nodes p and q. p,q :

[0025]

[0026] By minimizing the energy function, we get Figure 2 The classification segmentation method is the current best segmentation result. At this time, the points connected to the background points are marked as ground points, and the points connected to the objects are marked as obstacle points.

[0027] Preferably, the constraint algorithm in step S3 is specifically:

[0028] Step S31, current data collection scene recognition; the ground point cloud is uniformly projected into the current sensor coordinate system, and the Gauss-Newton method is used to solve the regression gradient k and intercept b of the equation for all projection points;

[0029] Step S32, fitting the regression equation to calculate the accuracy and obtain the root mean square error (RMSE); if the RMSE exceeds the pre-set threshold K u , that is, the error is considered too large, and the regression equation is refitted; if it is less than the threshold, k is used to judge the current scene;

[0030] Step S33, judging the three scenarios of going upstairs, going downstairs and on flat ground; if k is greater than the threshold k up It is a scene of going upstairs. If k is less than the threshold k down The scenario is going downstairs, and the rest are walking on flat ground.

[0031] Step S34, establishing multi-scenario constraint equations;

[0032] When switching from walking on flat ground to walking up and down stairs, the constraints are expressed by the following two equations:

[0033]

[0034] in, The type parameter represents the pose transformation matrix parameters from frame k-1 to frame k obtained through lidar ICP registration; represents the error function in the pose graph; represents the rotation matrix from k-1 to k, represents the key frame pose at time point j in odometry; Represents the translation matrix from K-1 to time k.

[0035] When switching from the staircase scene to the plane scene, the constraint is expressed as the following equation:

[0036]

[0037] in, is the translation in the Z-axis direction, and d is the offset in the Z-axis direction;

[0038] Step S35, adding existing constraints to the pose graph.

[0039] Preferably, the posture is performed by matrix multiplication.

[0040] Preferably, step S2, step S3, and step S4 are performed in a GPU graphics computing unit.

[0041] This application also proposes a multi-story building mapping system based on laser radar and IMU coupling, using the above-mentioned mapping method, including:

[0042] The data scanning and acquisition module is used to scan and collect environmental information, obtain point cloud data, use IMU input data to remove motion distortion of the lidar point cloud, and use the grid downsampling algorithm to downsample the point cloud data;

[0043] The ground point cloud removal module is used to remove the ground point cloud from the single-frame point cloud, highlight the key features of the point cloud data, and accumulate key frame data for the processed single-frame point cloud;

[0044] Pose constraint optimization module, which is used to construct optimized pose graphs using multi-plane ground constraint pose constraint algorithms;

[0045] The motion estimation compensation module is used to obtain the historical cumulative error value obtained by the current map construction, and combine the posture error values ​​from different posture sources; the error fusion algorithm based on Kalman filtering is used to calculate the fusion error value and compensate the motion estimation;

[0046] The global map fusion module is used to align and fuse the current key frame point cloud data with the accumulated global map of the previous moment through the point iterative ICP algorithm.

[0047] The present application also proposes a computer-readable storage medium on which a computer program is stored, and when the program is executed by a processor, the above-mentioned mapping method is implemented.

[0048] The multi-story building mapping method provided in this application has the following technical effects or advantages:

[0049] 1. This application removes single-frame ground points based on the Markov model, and utilizes the model-based mathematical abstract feature extraction of the ground to achieve accurate identification and extraction of the ground in multiple scenarios; at the same time, the single-frame ground points are removed to achieve real-time data processing, reduce the computational processing amount of output data, improve data processing speed, and reduce device storage.

[0050] 2. This application proposes a multi-plane-based ground constraint posture constraint algorithm. The algorithm constructs constraint equations suitable for three scenarios: going upstairs, going downstairs, and flat ground according to the multi-story building structure. It is aimed at the use scenarios of corridor scenes, up and down stairs, and multi-story similar floors similar to indoor mapping, and effectively curbs the mapping drift phenomenon and scene degradation phenomenon.

[0051] 3. Based on the overall operation flow and data processing architecture of this application, in terms of data volume, the point cloud is downsampled to effectively remove motion distortion; in terms of algorithm, the point cloud is processed using incremental kd trees and incremental Kalman filters to improve calculation accuracy and speed; in terms of code architecture, the GPU computing unit is used to accelerate the point cloud processing steps and reduce computing time through parallel processing. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] Figure 1 A flow chart of the method for mapping a multi-story building for this application;

[0053] Figure 2 This is a schematic diagram of data transmission for the multi-story building mapping method implemented in this application. DETAILED DESCRIPTION

[0054] The embodiments of the present application provide a multi-story building mapping method, system and medium based on laser radar and IMU coupling, which solves the problem of low modeling quality in the prior art due to the existence of similar structures and narrow structures in the process of multi-story indoor building structure modeling, improves the modeling model accuracy of the multi-story building structure through multiple ground scene constraints, and reduces error accumulation and improves calculation speed through algorithm optimization.

[0055] In order to better understand the above technical solution, the above technical solution will be described in detail below in conjunction with the accompanying drawings and specific implementation methods.

[0056] like Figure 1As shown, this embodiment discloses a multi-story building mapping method, system and medium based on laser radar and IMU coupling, and the specific steps are as follows:

[0057] In step S1, the laser radar sensor scans and collects environmental information to obtain point cloud data, uses IMU input data to remove motion distortion of the laser radar point cloud, and uses a grid downsampling algorithm to downsample the point cloud data.

[0058] The single scanning time of the laser radar is t s , the coordinates of a scanning point in the laser radar coordinate system are p = (x, y, z) T , where the scanning time of this point is t∈(0,t s )

[0059] Since a single-frame laser radar scan takes time, and the laser radar carrier may move during a single scan, it is necessary to remove motion distortion from the laser radar point cloud. By performing SLERP spherical interpolation on high-frequency IMU data, the IMU pose at time t can be obtained as T(t) WI , the IMU pose at the end of the scan is T(t s ) WI , after obtaining the external parameters T of the laser radar and IMU IL After that, the p′ after motion compensation at point p is at the end of scanning time t s The coordinates are:

[0060] p′=T LI T(t s ) IW T(t) WI T IL p t

[0061] In order to effectively reduce the dimensionality of single-frame point cloud data, this algorithm uses a grid downsampling algorithm to reduce the amount of single-frame data. Specifically, for point p i =(x i ,y i ,z i ), with a grid size of v x ,v y ,v z When the grid index Voxel(p i )=(i,j,k), where:

[0062]

[0063] For all points in the grid, form a point set:

[0064]

[0065] After calculating all the points in the point set, the centroid of all points is used to represent the point cloud data of the grid. Specifically:

[0066]

[0067] Step S2, based on the Markov random field, the ground point cloud is removed from the single-frame point cloud, the key features of the point cloud data are highlighted, the single-frame point cloud currently collected is integrated into the global map, and the historical cumulative map is completed; the ground point cloud removal of the single-frame point cloud includes expanding the point cloud ring according to the angle between each point and the z-axis, completing obstacle recognition according to the height difference between two points in the same ring, using the binary classification task of the graph to distinguish the ground point cloud, and completing ground point recognition by constraining the segmentation quality through the Markov random field.

[0068] Removing ground point clouds from single-frame point clouds can reduce a large amount of redundant and noisy data in the ground point clouds and improve the image quality of the final indoor 3D map. In addition, removing ground point clouds will also make the key features of the remaining point cloud data, such as indoor walls, obstacles, objects, etc., more prominent, improving the quality of subsequent algorithm processing.

[0069] In the bird's-eye view, the point cloud is expanded according to the angle between each point and the z-axis. According to the scanning characteristics of the laser radar, different laser beams will form multiple rings in the bird's-eye view;

[0070] Obstacle point recognition: In the obtained bird's-eye view, if the height difference and point cloud arrangement between two points in the same circle change, it can be simply determined that there is an obstacle between the two points. That is, when Δd is less than the threshold, it can be considered that there is an obstacle between the two points. Where h is the installation height of the lidar, δ 1 and δ 2 is the angle between the laser beam and the ground normal, and K is the maximum height difference and elevation angle that can be set.

[0071]

[0072] Ground point recognition: This algorithm projects the point cloud graph into a range graph, takes advantage of the close proximity of ground point clouds, uses the binary classification task of the graph to distinguish the ground point clouds, and constrains the segmentation quality through Markov random fields. Specifically, we use the energy function E(A) = λ·R(A) + B(A) to constrain the segmentation results, where R(A) is defined as the error penalty for node p being assigned to "background" or "object", and B(A) is defined as the penalty coefficient B defined as the boundary discontinuity between nodes p and q. p,q .

[0073]

[0074] Where R(A) is the local score R of all elements in set P p (A p ) total; A p Partition the region associated with element p;

[0075] At this time, by minimizing the energy function, we get Figure 2 The classification segmentation method is the current best segmentation result. At this time, the points connected to the "background" points are marked as ground points, and the points connected to the "object" are obstacle points: all point clouds are established as graph algorithms and divided into two categories: background points and object points. At the initial moment, high-confidence prior "background points" and "object points" are obtained. By minimizing the energy function, all points are continuously divided into these two types. Finally, the points connected to the "background" points are marked as ground points, and the points connected to the "object" are obstacle points.

[0076] Step S3, constructing an optimized pose graph based on a multi-plane ground constraint pose constraint algorithm.

[0077] This algorithm is mostly used for indoor multi-story building mapping. In multi-story indoor scenes, there are often similar narrow and unstructured spaces in the environment, such as corridors and stairs with similar textures. These similar scenes make the pose calculation of the lidar-IMU tightly coupled odometer very easy to drift, affecting the mapping effect. Therefore, this algorithm uses a multi-plane ground constraint pose constraint algorithm based on the ground point of each frame obtained in the previous step to improve the mapping quality in multiple similar scenes.

[0078] Current data collection scene recognition: For the ground point cloud obtained in the previous step, uniformly project it to the XY plane of the current sensor coordinate system, and use the Gauss-Newton method to solve the regression gradient k and intercept b of the equation for all projection points. The specific formula is as follows:

[0079]

[0080] in, and are the coordinates of the points, k is the slope of the regression equation, and b is the intercept of the regression equation. At this point, we will calculate the accuracy of the fitted regression equation and obtain the RMSE root mean square error. If the RMSE exceeds the pre-set threshold K u , that is, the error is too large, we will refit the regression equation; if it is less than the threshold, the slope k is used to judge the current scene: if k is greater than the threshold k set in advance up Then it can be judged as a scene of going upstairs, which is less than the threshold k down The scenario is going downstairs, and the rest are walking on flat ground.

[0081] Multi-ground scene constraints: When the scene switches from walking on flat ground to going up and down stairs, we can assume that a big change is about to occur in the z-axis direction, so only binary edges from the lidar odometer are added to the pose graph. The constraints from the lidar can be expressed by the following two equations:

[0082]

[0083] in, The type parameter represents the pose transformation matrix parameters from frame k-1 to frame k obtained through lidar ICP registration. represents the error function in the pose graph; represents the rotation matrix from k-1 to k, represents the key frame pose at time point j in odometry; Represents the translation matrix from K-1 to time k.

[0084] When switching from the staircase scene to the plane scene, the plane equation of the current ground can be constructed from the plane scene as follows:

[0085] ax i +by i +cz i +d=0

[0086] The normal vector of the ground is n = [a, b, c] T , the ground can be further standardized as:

[0087] n·p i +d=0

[0088] To further facilitate optimization and constraint construction, we align the normal vector to the z-axis. The equation is further simplified to, where d is the z-axis offset:

[0089] z i +d=0

[0090] In order to add the plane equation as a constraint to the pose graph optimization, given a pose T k (i.e. the rotation matrix R k With the translation vector t k When , the ground constraint is expressed as:

[0091] n T (R k p k +t k )+d=0

[0092] It can be further simplified to:

[0093] [0,0,1]·(R k p k +t k )+d=0

[0094] The final expression is:

[0095]

[0096] Therefore, our optimization goal at this time is to translate the robot in the Z-axis direction Adjusted to be consistent with the plane constant d, the motion on the plane is accurate.

[0097] Adding existing constraints to the pose graph: In order to construct the pose graph, the above constraints need to be added as edges to the graph. Suppose there are two poses T i and T j ,The final goal of the multi-plane ground constraint is written as:

[0098]

[0099] This error function is minimized by an optimizer (such as G2O) to obtain the best pose graph so that the motion trajectory conforms to the plane constraints in space.

[0100] Step S4: motion compensation estimation.

[0101] The back propagation module in the algorithm after data accumulation is used to calculate the historical cumulative error value obtained by the current map construction; at the same time, combined with the pose correction module based on multiple ground constraints obtained in the previous step, this algorithm once again obtains the error values ​​from different pose sources. Using the error fusion algorithm based on Kalman filtering, a more accurate pose error value is calculated and the motion estimation is compensated.

[0102] In the process of mapping, a large number of pose settlement steps are required. In the implementation of this algorithm, pose solution is usually achieved through matrix multiplication operations. If a single-threaded model is used for mapping, the main thread will spend a lot of time on pose solution. Since the GPU has a natural advantage in computationally intensive tasks of matrix operations, all pose solution and pose graph optimization steps of this algorithm are performed in the GPU graphics computing unit. In addition, sub-graph construction and global map updates are performed using multiple threads, which greatly improves CPU utilization and achieves extremely high real-time performance of the algorithm.

[0103] Step S5, the point cloud data of the current key frame is registered and fused with the accumulated global map of the previous moment through the point iteration ICP algorithm, and the newly added point cloud data is registered into the dynamic kd tree to speed up the subsequent point cloud registration steps.

[0104] This embodiment also proposes a multi-story building mapping system based on laser radar and IMU coupling, including:

[0105] The data scanning and acquisition module is used to scan and collect environmental information, obtain point cloud data, use IMU input data to remove motion distortion of the lidar point cloud, and use the grid downsampling algorithm to downsample the point cloud data;

[0106] The ground point cloud removal module is used to remove the ground point cloud from the single-frame point cloud, highlight the key features of the point cloud data, and accumulate key frame data for the processed single-frame point cloud;

[0107] Pose constraint optimization module, which is used to construct optimized pose graphs using multi-plane ground constraint pose constraint algorithms;

[0108] The motion estimation compensation module is used to obtain the historical cumulative error value obtained by the current map construction, and combine the posture error values ​​from different posture sources; the error fusion algorithm based on Kalman filtering is used to calculate the fusion error value and compensate the motion estimation;

[0109] The global map fusion module is used to align and fuse the current key frame point cloud data with the accumulated global map of the previous moment through the point iterative ICP algorithm.

[0110] This embodiment also proposes a computer-readable storage medium, on which a computer program is stored. When the program is executed by a processor, the above-mentioned mapping method is implemented.

[0111] In the above embodiments, it can be implemented in whole or in part by software, hardware, firmware or any combination thereof. When implemented by software, it can be implemented in whole or in part in the form of a computer program product. The above computer program product includes one or more computer instructions. When the computer program instructions are loaded and executed on a computer, the process or function described in the embodiment of the present invention is generated in whole or in part. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions can be stored in a computer-readable storage medium, or transmitted from one computer-readable storage medium to another computer-readable storage medium. For example, the computer instructions can be transmitted from one website site, computer, server or data center to another website site, computer, server or data center by wired (e.g., coaxial cable, optical fiber, digital subscriber line) or wireless (e.g., infrared, wireless, microwave, etc.) mode. The computer-readable storage medium can be any available medium that a computer can store or a data storage device such as a server or data center that includes one or more available media integrated. The available medium can be a magnetic medium (e.g., a floppy disk, a hard disk, a tape), an optical medium (e.g., a DVD), or a semiconductor medium (e.g., a solid-state hard disk (SSD)), etc.

[0112] The embodiments of this specific implementation mode are all preferred embodiments of the present invention, and are not intended to limit the protection scope of the present invention. Therefore, all equivalent changes made according to the structure, shape, and principle of the present invention should be included in the protection scope of the present invention. Although the preferred embodiments of the present invention have been described, once the technical personnel in this field know the basic creative concept, they can make other changes and modifications to these embodiments. Therefore, the attached claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present invention. Obviously, the technical personnel in this field can make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. In this way, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalent technologies, the present invention is also intended to include these changes and variations.

Claims

1. A multi-story building mapping method based on laser radar and IMU coupling, characterized in that: The following steps are involved: Step S1, the laser radar sensor scans and collects environmental information to obtain point cloud data, uses IMU input data to remove motion distortion of the laser radar point cloud, and uses a grid downsampling algorithm to downsample the point cloud data; Step S2, removing ground point clouds from single-frame point clouds based on Markov random fields, highlighting key features of point cloud data, and accumulating key frame data for the processed single-frame point clouds; removing ground point clouds from single-frame point clouds includes expanding each point cloud ring, completing obstacle recognition based on the height difference between two points in the same ring, using the binary classification task of the graph to distinguish the ground point clouds, and completing ground point recognition by constraining the segmentation quality through the Markov random field; Step S3, constructing an optimized pose graph based on a multi-plane ground constraint pose constraint algorithm; the constraint algorithm includes: uniformly projecting the ground point cloud into the current sensor coordinate system, fitting a regression equation to all projection points, judging three scenes of going upstairs, going downstairs, and flat ground; constructing constraints for the three scenes of going upstairs, going downstairs, and flat ground; adding the constraints as edges in the pose graph; Step S4, according to the back propagation module after the key frame data accumulation in step S2, the historical cumulative error value obtained by the current map is calculated; the pose error values ​​of different pose sources are obtained by combining the pose graph obtained by the constraint algorithm in step S3; the fusion error value is calculated by using the error fusion algorithm based on Kalman filtering, and the motion estimation is compensated; Step S5, the point cloud data of the current key frame is registered and fused with the accumulated global map of the previous moment through the point iteration ICP algorithm, and the newly added point cloud data is registered into the dynamic kd tree to speed up the subsequent point cloud registration steps.

2. The multi-story building mapping method according to claim 1, characterized in that: In step S1, the point cloud data is downsampled using a grid downsampling algorithm, specifically: For point p i =(x i ,y i ,z i ), with a grid size of v x ,v y ,v z When the grid index Voxel(p i )=(i,j,k), where: For all points in the grid, form a point set: After calculating all the points in the point set, the centroid of all points is used to represent the point cloud data of the grid, which is specifically expressed as:

3. The multi-story building mapping method according to claim 1, characterized in that: The obstacle identification in step S2 is specifically as follows: When the height difference Δd between two points in the same circle is less than the threshold, there is an obstacle between the two points: Where h is the installation height of the lidar, δ1 and δ2 are the angles between the laser beam and the ground normal, and K is the maximum height difference and elevation angle set.

4. The multi-story building mapping method according to claim 1, characterized in that: The ground point identification in step S2 is specifically as follows: The energy function E(A) = λ·R(A) + B(A) is used to constrain the segmentation results, where R(A) is defined as the error penalty for node p being assigned to the background or object, and B(A) is defined as the penalty coefficient B defined as the boundary discontinuity between nodes p and q. p,q : By minimizing the energy function, the image binary classification segmentation method obtained is the current best segmentation result. At this time, the points connected to the background points are marked as ground points, and the points connected to the objects are obstacle points.

5. The multi-story building mapping method according to claim 1, characterized in that: The constraint algorithm in step S3 is specifically: Step S31, current data collection scene recognition; the ground point cloud is uniformly projected into the current sensor coordinate system, and the Gauss-Newton method is used to solve the regression gradient k and intercept b of the equation for all projection points; Step S32, fitting the regression equation to calculate the accuracy and obtain the root mean square error (RMSE); if the RMSE exceeds the pre-set threshold K u , that is, the error is considered too large, and the regression equation is refitted; if it is less than the threshold, k is used to judge the current scene; Step S33, judging the three scenarios of going upstairs, going downstairs and on flat ground; if k is greater than the threshold k up It is a scene of going upstairs. If k is less than the threshold k down The scenario is going downstairs, and the rest are walking on flat ground. Step S34, establishing multi-scenario constraint equations; When switching from walking on flat ground to walking up and down stairs, the constraints are expressed by the following two equations: in, The type parameter represents the pose transformation matrix parameters from frame k-1 to frame k obtained through lidar ICP registration; represents the error function in the pose graph; represents the rotation matrix from k-1 to k, represents the key frame pose of point j at time k in the odometry coordinate system; Represents the translation matrix from time k-1 to time k; When switching from the staircase scene to the plane scene, the constraint is expressed as the following equation: in, is the translation in the Z-axis direction, and d is the offset in the Z-axis direction; Step S35, adding existing constraints to the pose graph.

6. The multi-story building mapping method according to claim 1, characterized in that: The pose is calculated by matrix multiplication.

7. The multi-story building mapping method according to claim 1, characterized in that: The steps S2, S3 and S4 are performed in a GPU graphics computing unit.

8. A multi-story building mapping system based on laser radar and IMU coupling, characterized in that: The mapping method according to any one of claims 1 to 7 comprises: The data scanning and acquisition module is used to scan and collect environmental information, obtain point cloud data, use IMU input data to remove motion distortion of the lidar point cloud, and use the grid downsampling algorithm to downsample the point cloud data; The ground point cloud removal module is used to remove the ground point cloud from the single-frame point cloud, highlight the key features of the point cloud data, and accumulate key frame data for the processed single-frame point cloud; Pose constraint optimization module, which is used to construct optimized pose graphs using multi-plane ground constraint pose constraint algorithms; The motion estimation compensation module is used to obtain the historical cumulative error value obtained by the current map construction, and combine the posture error values ​​from different posture sources; the error fusion algorithm based on Kalman filtering is used to calculate the fusion error value and compensate the motion estimation; The global map fusion module is used to align and fuse the current key frame point cloud data with the accumulated global map of the previous moment through the point iterative ICP algorithm.

9. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the mapping method according to any one of claims 1 to 7 is implemented.

Images