Indoor three-dimensional environment map construction method and device based on laser SLAM

Abstract

The invention provides an indoor three-dimensional environment map construction method and device based on laser SLAM, and the method comprises the steps: carrying out the time-space synchronization processing of laser data and IMU data, and carrying out the correction or distortion removal processing of the synchronized laser data; then determining a gravity direction in an equipment coordinate system by using data of the IMU equipment at a static moment, and then performing normal vector screening and clustering processing on feature points according to the gravity direction to obtain a candidate normal vector set; removing invalid feature points by using an affiliation relationship between normal vectors of feature points of the dedistorted laser data and a candidate normal vector set and a distance between each feature point and a laser radar center, screening to obtain effective feature points, and determining weights of the effective feature points; and then optimizing the positions and postures of the effective feature points by constructing a line feature residual equation and a surface feature residual equation, and dynamically updating the map feature points.

Description

Technical Field

[0001] The present invention relates to the field of real-time positioning and map construction, and in particular to the technical field of real-time positioning and map construction in an indoor three-dimensional environment, and discloses a method and a device for constructing an indoor three-dimensional environment map based on laser SLAM. Background Art

[0002] With the widespread application of mobile robots and automatic navigation systems, simultaneous localization and mapping (SLAM) technology plays a vital role in the autonomous navigation of robots. Laser SLAM has become one of the mainstream technologies due to its high precision and reliability, and is widely used in warehouse management, service robots and other fields. However, most current laser SLAM algorithms still face many challenges when facing complex indoor environments.

[0003] First, most existing SLAM algorithms have not been specifically adjusted for the characteristics of indoor three-dimensional environments. For example, the normal information of the ground and facades is not fully utilized, which makes it difficult for SLAM algorithms to effectively distinguish and utilize feature points of different planes in indoor environments with obvious three-dimensional structures, thus affecting the accuracy and robustness of mapping.

[0004] Secondly, in narrow areas such as small rooms and stairs, the space is limited and the point cloud distance of laser scanning is basically within 3 meters. In this close-range scanning environment, the point cloud data of dynamic targets and static targets are difficult to effectively distinguish through the point cloud features themselves. In addition, close-range scanning will also generate a large number of laser noise points, further exacerbating the complexity of point cloud data. Traditional distance filtering methods will cause many valid points to be misfiltered in such environments, making it impossible to complete effective mapping; if no filtering is performed, the interference of dynamic targets and noise points will cause mapping failure or a significant decrease in accuracy.

[0005] There is an urgent need for a method that can effectively filter unstable features in laser point clouds according to the characteristics of indoor stereo environments, so as to improve the robustness and accuracy of SLAM systems in complex indoor environments. Summary of the invention

[0006] The present disclosure at least provides a method and device for constructing an indoor stereoscopic environment map based on laser SLAM to solve the problems in the prior art of laser SLAM in complex indoor environments, such as small rooms, stairs and other narrow areas, due to the short point cloud distance, difficulty in distinguishing dynamic targets from static targets, and laser noise interference, which leads to low robustness and accuracy of mapping.

[0007] According to one aspect of the present disclosure, a method for constructing an indoor stereoscopic environment map based on laser SLAM is provided, comprising:

[0008] Scanning data of the indoor space using a laser radar to obtain laser data, and using an IMU device to obtain environmental data of the indoor space to obtain IMU data; performing spatiotemporal synchronization processing on the laser data and the IMU data to obtain target laser data and target IMU data;

[0009] Extracting data of the IMU device at rest from the target IMU data, and determining the gravity direction in the device coordinate system corresponding to the IMU device based on the extracted data;

[0010] For each frame of target laser data in the target laser data, the target IMU data with the same time as the current frame of target laser data is used to determine the posture data of the IMU device, the posture data of the laser radar is determined according to the posture data of the IMU device, the posture data of the laser radar is interpolated, and the current frame of target laser data is converted to the global coordinate system to obtain dedistorted laser data, and the dedistorted laser data is converted to the posture data of the laser radar at the end of the current frame to obtain target dedistorted laser data; feature points of the target dedistorted laser data are extracted; the feature points include feature line points and feature surface points;

[0011] Using the first frame of target dedistorted laser data, an initial map point cloud is constructed through the feature line points and feature surface points of the first frame of target dedistorted laser data;

[0012] For each frame of target dedistorted laser data after the first frame of target dedistorted laser data, determine the normal vectors of each feature point corresponding to the current frame of target dedistorted laser data, and cluster the normal vectors parallel to the gravity direction in the normal vectors to determine a set of candidate normal vectors; determine valid feature points and the weight of each valid feature point according to the normal vectors of each feature point corresponding to the current frame of target dedistorted laser data, the distance between each feature point and the center of the laser radar, and the set of candidate normal vectors; and construct the line feature residual equation and the surface feature residual equation corresponding to the current frame of target dedistorted laser data using the weight and normal vector of each feature point in the valid feature points;

[0013] The optimized position and optimized posture of each valid feature point are determined by using each line feature residual equation and each surface feature residual equation; the optimized position and optimized posture of each valid feature point are checked for validity; after the validity check is passed, each valid feature point is added to the initial map point cloud according to the optimized position and optimized posture of each valid feature point.

[0014] In a possible implementation, the posture data of the IMU device and the posture data of the laser radar have the same frequency;

[0015] The interpolation processing of the attitude data of the laser radar includes:

[0016] The timestamp of each laser point in the target laser data of the current frame is interpolated, and the attitude data corresponding to each interpolated timestamp is determined according to the attitude data of the laser radar.

[0017] In a possible implementation manner, determining the normal vector of each feature point corresponding to the dedistorted laser data of the target in the current frame includes:

[0018] For each feature point corresponding to the dedistorted laser data of the target in the current frame, the adjacent points of the feature point are searched in the initial map point cloud, and the normal vector of the feature point is determined using the searched adjacent points.

[0019] In a possible implementation, determining valid feature points and weights of each valid feature point according to the normal vector of each feature point corresponding to the dedistorted laser data of the current frame target, the distance between each feature point and the center of the laser radar, and the set of candidate normal vectors includes:

[0020] For each feature point corresponding to the dedistorted laser data of the target in the current frame, if the normal vector of the feature point does not belong to the set of candidate normal vectors, the distance between the feature point and the center of the laser radar is less than the first preset distance, and it does not belong to the vector perpendicular to the direction of gravity, then the feature point is determined to be an invalid feature point and the feature point is eliminated; if the normal vector of the feature point does not belong to the set of candidate normal vectors, and the distance between the feature point and the center of the laser radar is less than the second preset distance, then the feature point is determined to be a valid feature point, and a weight value within a first numerical range is set for the feature point; if the distance between the feature point and the center of the laser radar is greater than the second preset distance, then the feature point is determined to be a valid feature point, and a weight value within a second numerical range is set for the feature point; wherein the weight value within the first numerical range is less than the weight value within the second numerical range.

[0021] In a possible implementation, the weights and normal vectors of each feature line point in the effective feature points are used to construct the line feature residual equation corresponding to each frame of target dedistorted laser data, including:

[0022] The line feature residual equation corresponding to each frame of target dedistorted laser data is constructed using the following formulas:

[0023]

[0024] Wherein, i represents the data frame after the first frame of target de-distorted laser data, m represents the value obtained by minus 1 of the frame number of target de-distorted laser data; k represents the feature line extracted from one frame of target de-distorted laser data, h represents the number of feature lines extracted from one frame of target de-distorted laser data; j represents the feature line point on the feature line, and n represents the number of feature line points on one feature line; L k is the extracted line parameter, R i is the pose of the dedistorted laser data of the i-th frame target to be optimized, P j is the line feature of the jth feature line point, W j is the weight value corresponding to the current feature line point.

[0025] In a possible implementation, the weights and normal vectors of each feature surface point in the effective feature points are used to construct the surface feature residual equation corresponding to each frame of target dedistorted laser data, including:

[0026] The surface feature residual equation corresponding to each frame of target dedistorted laser data is constructed using the following formulas:

[0027]

[0028] Wherein, i represents the data frame after the first frame of target dedistortion laser data, m represents the value obtained by minus 1 of the frame number of target dedistortion laser data; l represents the feature surface extracted from one frame of target dedistortion laser data, f represents the number of feature surfaces extracted from one frame of target dedistortion laser data; b represents the feature surface points on the feature surface, and v represents the number of feature surface points on one feature surface; N l is the extracted plane parameter, R i is the pose of the dedistorted laser data of the i-th frame target to be optimized, P b is the surface feature of the extracted feature points, W b is the weight value corresponding to the current feature point.

[0029] In a possible implementation, the use of each line feature residual equation and each surface feature residual equation to determine the optimized position and optimized pose of each effective feature point includes:

[0030] Through the nonlinear optimization algorithm, the residual equations of each line feature and each surface feature residual equation are processed to obtain the optimized position and optimized posture of each effective feature point.

[0031] In a possible implementation, the checking the validity of the optimized position and optimized posture of each valid feature point includes:

[0032] The optimized position of each valid feature point is used to determine the optimized residual value. If the difference between the optimized residual value and the residual value before optimization determined by the feature points before optimization meets the preset requirements and the number of valid feature points is greater than the preset number of points, the validity check is determined to have passed.

[0033] In a possible implementation manner, after the validity check passes, the method further includes:

[0034] The normal vector of each valid feature point in the initial map point cloud is calculated respectively.

[0035] According to another aspect of the present disclosure, a device for constructing an indoor stereoscopic environment map based on laser SLAM is provided, comprising:

[0036] A data acquisition synchronization module is used to use a laser radar to scan the indoor space to obtain laser data, and use an IMU device to obtain environmental data of the indoor space to obtain IMU data; the laser data and the IMU data are processed in time and space synchronization to obtain target laser data and target IMU data;

[0037] A gravity direction determination module, used to extract data of the IMU device at a stationary moment from the target IMU data, and determine the gravity direction in the device coordinate system corresponding to the IMU device based on the extracted data;

[0038] A dedistortion module is used to determine the posture data of the IMU device for each frame of target laser data in the target laser data by using the target IMU data with the same time as the current frame of target laser data, determine the posture data of the laser radar according to the posture data of the IMU device, perform interpolation processing on the posture data of the laser radar, and convert the current frame of target laser data to the global coordinate system to obtain dedistorted laser data, convert the dedistorted laser data to the posture data of the laser radar at the end of the current frame to obtain target dedistorted laser data; extract feature points of the target dedistorted laser data; the feature points include feature line points and feature surface points;

[0039] An initial map construction module is used to use the first frame of target dedistorted laser data to construct an initial map point cloud through feature line points and feature surface points of the first frame of target dedistorted laser data;

[0040] A map updating module is used to determine the normal vectors of each feature point corresponding to the current frame target dedistorted laser data for each frame target dedistorted laser data after the first frame target dedistorted laser data, and cluster the normal vectors in the normal vectors parallel to the gravity direction to determine a set of candidate normal vectors; determine valid feature points and the weights of each valid feature point according to the normal vectors of each feature point corresponding to the current frame target dedistorted laser data, the distances between each feature point and the center of the laser radar, and the set of candidate normal vectors; construct the line feature residual equation and the surface feature residual equation corresponding to the current frame target dedistorted laser data respectively using the weights and normal vectors of each feature point among the valid feature points; and determine the optimized position and optimized posture of each valid feature point using each line feature residual equation and each surface feature residual equation; perform validity check on the optimized position and optimized posture of each valid feature point; after the validity check is passed, add each valid feature point to the initial map point cloud according to the optimized position and optimized posture of each valid feature point.

[0041] The disclosed method and device for constructing an indoor stereoscopic environment map based on laser SLAM performs spatiotemporal synchronization processing on laser data and IMU data, and performs deviation correction or distortion removal processing on the synchronized laser data, which significantly improves the accuracy and consistency of the laser data used for subsequent map construction, thereby enhancing the SLAM positioning accuracy and map construction quality. The disclosed method uses the data at the moment when the IMU device is stationary to determine the gravity direction in the device coordinate system, and then screens and clusters the normal vectors of the feature points according to the gravity direction to obtain a set of candidate normal vectors, and then uses the normal vectors of the feature points of the dedistorted laser data and the attached relationship with the candidate normal vector set, and the distance between each feature point and the center of the laser radar to eliminate invalid feature points, and screen out valid feature points. The disclosed solution performs feature screening on the normal information of indoor floors and facades, and obtains a set of candidate normal vectors. Subsequent processing using the candidate normal vector set can more effectively identify and utilize complex indoor three-dimensional structures, improve the robustness and accuracy of mapping, and is particularly suitable for indoor environments with obvious three-dimensional features. In addition, the above-mentioned solution designs a feature screening strategy for small rooms, stairs and other narrow areas, removes invalid feature points, and can effectively distinguish dynamic targets from static targets and laser noise points, avoiding the mistaken abandonment of valid points due to traditional distance filtering, thereby ensuring stable operation of the solution and high-precision mapping in complex and narrow environments. While eliminating invalid feature points and screening out valid feature points, the present disclosure determines the weight of the valid feature points based on the distance between the feature points and the center of the laser radar, the affiliation between the normal vector of the feature points and the candidate normal vector set, and dynamically adjusts the weight of the valid feature points according to the characteristics of different indoor environments, thereby enhancing the adaptability and flexibility of the solution, enabling it to maintain excellent performance in a variety of indoor scenes, and setting weights by comprehensively considering the normal information of the feature points and the distance between the feature points and the laser radar, which can effectively filter out laser noise points and dynamic interference, improve the overall quality of the data, and enhance the robustness and reliability of the solution. In addition, the present disclosure optimizes the position and posture of the valid feature points by constructing line feature residual equations and surface feature residual equations, and dynamically updates the feature points of the map on this basis, ensuring the real-time and accuracy of the map, and supporting the construction of SLAM maps with long-term stable operation.

[0042] It should be understood that the content described in this section is not intended to identify the key or important features of the embodiments of the present disclosure, nor is it intended to limit the scope of the present disclosure. Other features of the present disclosure will become easily understood through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] The accompanying drawings are used to better understand the present solution and do not constitute a limitation of the present disclosure.

[0044] Figure 1is a flow chart of a method for constructing an indoor stereoscopic environment map based on laser SLAM according to an embodiment of the present disclosure;

[0045] Figure 2 It is a structural schematic diagram of an indoor stereoscopic environment map construction device based on laser SLAM according to an embodiment of the present disclosure;

[0046] Figure 3 It is a schematic diagram of the structure of an electronic device according to an embodiment of the present disclosure. DETAILED DESCRIPTION

[0047] The following is a description of exemplary embodiments of the present disclosure in conjunction with the accompanying drawings, including various details of the embodiments of the present disclosure to facilitate understanding, which should be considered as merely exemplary. Therefore, it should be recognized by those of ordinary skill in the art that various changes and modifications may be made to the embodiments described herein without departing from the scope and spirit of the present disclosure. Similarly, for the sake of clarity and conciseness, descriptions of well-known functions and structures are omitted in the following description.

[0048] The present invention is based on the problem that the laser SLAM in the prior art has low robustness and accuracy in mapping due to the short distance of point clouds, the difficulty in distinguishing between dynamic and static targets, and the interference of laser noise in complex indoor environments, such as small rooms and stairs. A method and device for building indoor stereo environment maps based on laser SLAM are proposed. The scheme of the present invention realizes the screening of unstable features of laser synchronous positioning and mapping (SLAM) of indoor stereo environment, and updates the constructed map on this basis; specifically, the present invention extracts feature line points and feature surface points through the spatiotemporal synchronization and correction processing of laser data and inertial measurement unit (IMU) data, and screens and filters unstable features based on normal information and distance parameters, especially for small indoor rooms, stairs and other narrow areas, solving the problem of low robustness and accuracy of mapping caused by traditional SLAM algorithms in these environments due to the short distance of point clouds, the difficulty in distinguishing between dynamic and static targets, and the interference of laser noise. Experimental results show that the scheme of the present invention significantly improves the accuracy of laser mapping and the reliability of positioning in complex indoor environments, and has broad application prospects.

[0049] The technical solution of the present disclosure is described below through specific embodiments.

[0050] like Figure 1 As shown, it is a flow chart of the method for constructing an indoor stereoscopic environment map based on laser SLAM of this embodiment. The execution subject of this embodiment is a component, device or system with computing capability. Specifically, the method for constructing an indoor stereoscopic environment map based on laser SLAM of this embodiment includes the following steps:

[0051] S110, using a laser radar to scan data of an indoor space to obtain laser data, using an IMU device to obtain environmental data of the indoor space to obtain IMU data; performing spatiotemporal synchronization processing on the laser data and the IMU data to obtain target laser data and target IMU data.

[0052] Use lidar and IMU devices to simultaneously collect data on indoor environments. Through timestamp and spatial calibration methods, the two data sources are synchronized in time and space to ensure data consistency.

[0053] S120, extracting data of the IMU device at a stationary moment from the target IMU data, and determining the gravity direction in the device coordinate system corresponding to the IMU device based on the extracted data.

[0054] This step can be implemented using the following steps:

[0055] (1) Identify the moment when the IMU device is stationary and extract the IMU data at that moment, where the accelerometer data mainly reflects the direction of gravity and the gyroscope data reflects the angular velocity of the device.

[0056] (2) Use the Kalman filter to process the IMU data at rest, fuse the IMU data, filter out noise, and provide stable gravity direction estimation data.

[0057] (3) Based on the output of the Kalman filter, the direction of gravity in the IMU device coordinate system is determined to provide a reference for subsequent ground horizontal and wall vertical constraints.

[0058] S130, perform de-distortion processing on the target laser data, and extract feature points of the processed laser data: for each frame of target laser data in the target laser data, use the target IMU data with the same time as the current frame of target laser data to determine the posture data of the IMU device, determine the posture data of the laser radar according to the posture data of the IMU device, interpolate the posture data of the laser radar, and convert the current frame of target laser data to the global coordinate system to obtain de-distorted laser data, convert the de-distorted laser data to the posture data of the laser radar at the end of the current frame to obtain target de-distorted laser data; extract feature points of the target de-distorted laser data; the feature points include feature line points and feature surface points.

[0059] For each frame of target IMU data, its posture data includes some columns of data and is not limited to only one data. Similarly, the posture data of each frame of target laser data also includes some columns of data; and the posture data of the IMU device has the same frequency as the posture data of the laser radar.

[0060] In this step, the laser data can be rectified based on the attitude, acceleration and other information in the IMU data to eliminate the errors caused by motion and improve the accuracy and consistency of the laser data.

[0061] In this step, edge detection algorithm and plane fitting algorithm can be used to extract feature line points and feature surface points from the corrected laser data as basic features for subsequent matching and optimization.

[0062] S140: Use the first frame of target dedistorted laser data to construct an initial map point cloud through the feature line points and feature surface points of the first frame of target dedistorted laser data.

[0063] Using the first frame of laser data after deflection processing, the initial map point cloud is constructed through feature line points and feature surface points.

[0064] S150. Generate a set of candidate normal vectors based on the dedistorted laser data, then screen the feature points, construct the residual equation, optimize the position and posture of the feature points, and update them to the initial map point cloud: for each frame of target dedistorted laser data after the first frame of target dedistorted laser data, determine the normal vectors of each feature point corresponding to the current frame of target dedistorted laser data, and cluster the normal vectors parallel to the gravity direction in the normal vectors to determine a set of candidate normal vectors; based on the normal vectors of each feature point corresponding to the current frame of target dedistorted laser data, the distance between each feature point and the center of the laser radar, and the set of candidate normal vectors , determine the valid feature points and the weight of each valid feature point; use the weight and normal vector of each feature point in the valid feature points to respectively construct the line feature residual equation and the surface feature residual equation corresponding to the dedistorted laser data of the current frame target; and use each line feature residual equation and each surface feature residual equation corresponding to the dedistorted laser data of each frame target to determine the optimized position and optimized posture of each valid feature point; perform a validity check on the optimized position and optimized posture of each valid feature point; after the validity check is passed, add each valid feature point to the initial map point cloud according to the optimized position and optimized posture of each valid feature point.

[0065] The above-mentioned determination of the normal vectors of each feature point corresponding to the dedistorted laser data of the current frame target can be achieved by the following steps:

[0066] For each feature point corresponding to the dedistorted laser data of the target in the current frame, the adjacent points of the feature point are searched in the initial map point cloud, and the normal vector of the feature point is determined using the searched adjacent points.

[0067] The specific implementation steps are as follows:

[0068] (1) For each feature point of each subsequent frame of target dedistorted laser data after the first frame, a neighboring point search is performed with the initial map point cloud to ensure that each feature point has sufficient neighboring support.

[0069] (2) The normal vector of each feature point is calculated through principal component analysis based on the surrounding adjacent points. The normal information perpendicular to the gravity direction and parallel to the gravity direction can also be counted for subsequent normal clustering analysis.

[0070] The above-mentioned clustering of the normal vectors parallel to the gravity direction in the normal vectors to determine the candidate normal vector set can be specifically implemented by the following steps: clustering analysis is performed on the normal vectors parallel to the gravity direction, and the normal vectors with a larger number of points are extracted as the candidate normal vector set. The candidate normal set represents the main planar structural features.

[0071] The above-mentioned determination of the valid feature points and the weight of each valid feature point according to the normal vector of each feature point corresponding to the dedistorted laser data of the current frame target, the distance between each feature point and the center of the laser radar, and the candidate normal vector set can be implemented by the following steps:

[0072] For each feature point corresponding to the dedistorted laser data of the target in the current frame, if the normal vector of the feature point does not belong to the set of candidate normal vectors, the distance between the feature point and the center of the laser radar is less than the first preset distance, and it does not belong to the vector perpendicular to the direction of gravity, then the feature point is determined to be an invalid feature point and the feature point is eliminated; if the normal vector of the feature point does not belong to the set of candidate normal vectors, and the distance between the feature point and the center of the laser radar is less than the second preset distance, then the feature point is determined to be a valid feature point, and a weight value within a first numerical range is set for the feature point; if the distance between the feature point and the center of the laser radar is greater than the second preset distance, then the feature point is determined to be a valid feature point, and a weight value within a second numerical range is set for the feature point; wherein the weight value within the first numerical range is less than the weight value within the second numerical range.

[0073] Specifically, the steps to set the weights are as follows:

[0074] (1) Dynamic target and noise point filtering: If the normal vector of a feature point does not belong to the candidate normal vector set, and its distance to the laser radar is less than the threshold A, and it does not belong to the vector perpendicular to the gravity direction, then the feature point is considered to be a dynamic target or laser noise point and is discarded.

[0075] (2) Low weight for static wall targets: If the distance between the feature point and the lidar is less than the threshold B and its normal vector does not belong to the candidate normal vector set, the feature point is considered to be a static wall target and a lower weight is set.

[0076] (3) High weight for effective targets: If the distance between a feature point and the lidar is greater than the threshold B, a higher weight is set for the feature point.

[0077] Using the weights and normal vectors of each feature line point in the effective feature points, the line feature residual equation corresponding to each frame of target dedistorted laser data is constructed respectively, and it is implemented according to the following formula:

[0078]

[0079] Wherein, i represents the data frame after the first frame of target de-distorted laser data, m represents the value obtained by minus 1 of the frame number of target de-distorted laser data; k represents the feature line extracted from one frame of target de-distorted laser data, h represents the number of feature lines extracted from one frame of target de-distorted laser data; j represents the feature line point on the feature line, and n represents the number of feature line points on one feature line; L k is the extracted line parameter, R i is the pose of the dedistorted laser data of the i-th frame target to be optimized, P j is the line feature of the jth feature line point, W j is the weight value corresponding to the current feature line point.

[0080] Among them, since the target dedistortion laser data of the current frame needs to match the target dedistortion laser data of the previous frame, there are m frames in total, and the 0th to mth frames in the window need to be optimized, and i represents each frame in the window; k is each feature line extracted from each frame of target dedistortion laser data. There are h feature lines extracted from each frame, and the 0th to hth feature lines need to be used in the optimization, and k represents each feature line in the optimization; there will be multiple feature line points on each feature line, j represents each feature line point, and 0 to n feature line points will be extracted from each frame of point cloud to participate in the optimization.

[0081] The line parameter represents the digital expression of a straight line in space, which is generally a three-dimensional point in space (P0 x ,P0 y ,P0 z ) and the three-dimensional vector (V x ,V y ,V z ) expression; other three-dimensional points in space (P x , P y , P z )If it is on a straight line, it must satisfy (P x -P0 x ) / V x =(P y -P0 y ) / V y =(P z -P0z ) / V z .

[0082] Using the weights and normal vectors of each feature point in the effective feature points, the surface feature residual equation corresponding to each frame of target dedistorted laser data is constructed respectively, and it is implemented according to the following formula:

[0083]

[0084] Wherein, i represents the data frame after the first frame of target dedistortion laser data, m represents the value obtained by minus 1 of the frame number of target dedistortion laser data; l represents the feature surface extracted from one frame of target dedistortion laser data, f represents the number of feature surfaces extracted from one frame of target dedistortion laser data; b represents the feature surface points on the feature surface, and v represents the number of feature surface points on one feature surface; N l is the extracted plane parameter, R i is the pose of the dedistorted laser data of the i-th frame target to be optimized, P b is the surface feature of the extracted feature points, W b is the weight value corresponding to the current feature point.

[0085] Among them, the plane parameter is expressed as a spatial plane, which can be controlled by four parameters: A*P x +B*P y +D*P z +D=0, where A, B, C, D are plane parameters, P x ,P y ,P z is a point in three-dimensional space that satisfies the above formula, expressing that this point is on the plane.

[0086] Using the residual equations of each line feature and each surface feature, the optimal position and pose of each effective feature point are determined, including:

[0087] Through the nonlinear optimization algorithm, the residual equations of each line feature and each surface feature residual equation are processed to obtain the optimized position and optimized posture of each effective feature point.

[0088] Specifically, according to the line feature residual and the surface feature residual set, the nonlinear least squares method is used for iteration, and the residual set can be expressed as:

[0089]

[0090] Where φ is the preset paradigm. By solving the function and optimizing the residual in the set, the final pose R to be optimized is obtained. i The ceres solver can be used to set the number of iterations for multiple iterative solutions.

[0091] By combining the line and surface residuals for optimization, the optimized pose data of each frame can be obtained.

[0092] In some embodiments, the above-mentioned validity check of the optimized position and optimized posture of each valid feature point can be implemented by the following steps:

[0093] The optimized position of each valid feature point is used to determine the optimized residual value. If the difference between the optimized residual value and the residual value before optimization determined by the feature points before optimization meets the preset requirements and the number of valid feature points is greater than the preset number of points, the validity check is determined to have passed.

[0094] The specific steps are as follows:

[0095] Use the optimized position of each effective feature point to evaluate whether the residual value is significantly reduced after optimization, and check whether the number of effective feature points involved in the optimization exceeds the preset threshold. If the residual value is significantly reduced after the optimization and the number of points is sufficient, accept the optimization result. If the number of points involved in the optimization is small, adjust the threshold A, reduce the number of discarded points, and re-optimize until the optimization result meets the requirements.

[0096] In some embodiments, after the validity check is passed, the following steps may also be included:

[0097] The normal vectors of the effective feature points in the initial map point cloud are calculated respectively, and the optimized positions and postures of the effective feature points are registered with the initial point cloud map.

[0098] The optimized line feature points and surface feature points are recalculated in the initial map point cloud and added to the initial map point cloud to enhance the number and quality of feature points on the map and ensure real-time updating and accuracy of the map.

[0099] In some embodiments, the interpolation processing of the attitude data of the laser radar can be implemented by the following steps:

[0100] The timestamp of each laser point in the target laser data of the current frame is interpolated, and the attitude data corresponding to each interpolated timestamp is determined according to the attitude data of the laser radar.

[0101] The specific implementation steps of step S130 are as follows:

[0102] (1) According to the data collection time of the lidar, the target IMU data of the same time period is obtained, including information such as the acceleration and angular velocity measurement values ​​of the IMU.

[0103] (2) Filter the target IMU data and use a Kalman filter to remove noise.

[0104] (3) The attitude (rotation and translation) of the IMU during the scan is estimated by integrating the angular velocity measurement value (solving the direction) and doubly integrating the acceleration (solving the position), that is, the attitude data of the IMU device. Through the calibration relationship between the IMU device and the lidar, the attitude data of the lidar with the same frequency as the IMU device at the same time is obtained.

[0105] (4) According to the high-frequency laser radar attitude data obtained, the timestamp of each laser point in the current frame is interpolated, and the attitude data of the interpolation point is determined. The specific interpolation method can use linear interpolation or curve fitting interpolation.

[0106] (5) Apply a transformation matrix to the data of each laser point and transfer it to the global coordinate system to correct any deviation caused by device motion, and uniformly transfer the data of each laser point in the global coordinate system to the posture data at the end of the current laser radar frame.

[0107] Compared with the prior art, the solution of the above embodiment has at least one of the following advantages:

[0108] 1. Combining IMU data for point cloud correction: By performing spatiotemporal synchronization and correction processing on laser data and inertial measurement unit (IMU) data, the accuracy and consistency of laser data are significantly improved, thereby enhancing the positioning accuracy and mapping quality of SLAM composition.

[0109] 2. Make full use of the characteristics of indoor three-dimensional environment: This disclosure performs feature screening on the normal information of indoor floors and facades, which can more effectively identify and utilize complex indoor three-dimensional structures, improve the robustness and accuracy of mapping, and is particularly suitable for indoor environments with obvious three-dimensional features.

[0110] 3. Optimize mapping effects in narrow areas: A feature screening strategy is designed for narrow areas such as small rooms and stairs, which can effectively distinguish dynamic targets from static targets and laser noise points, avoid the mistaken abandonment of valid points due to traditional distance filtering, and ensure stable operation and high-precision mapping in complex and narrow environments.

[0111] 4. Dynamically adjust the weight of feature points: According to the characteristics of different indoor environments, the weight coefficients of feature points are dynamically adjusted to enhance the adaptability and flexibility of the method, enabling it to maintain excellent performance in a variety of indoor scenarios.

[0112] 5. Enhanced anti-noise capability: By comprehensively considering the normal information of the feature points and the distance from the sensor, the disclosed solution can effectively filter out laser noise points and dynamic interference, improve the overall quality of point cloud data, and enhance the robustness and reliability of the solution.

[0113] 6. Real-time optimization and map update: The disclosed solution optimizes the position and posture of feature points by constructing residual equations, and dynamically updates map feature points to ensure the real-time and accuracy of the map, supporting the execution of the solution for long-term stable operation.

[0114] The above advantages make the solution of the present disclosure have significant technical advantages in complex indoor environments, and can effectively solve the shortcomings of existing laser SLAM methods in terms of accuracy, robustness and adaptability, and improve the overall performance of indoor navigation and robot positioning systems.

[0115] Based on the same inventive concept, the present disclosure provides a device for constructing an indoor stereoscopic environment map based on laser SLAM, and the steps performed by the components of the device are the same or similar to those of the above method, so similar parts are not repeated. Figure 2 As shown, the indoor three-dimensional environment map construction device based on laser SLAM of this embodiment includes:

[0116] The data acquisition synchronization module 210 is used to use a laser radar to scan the indoor space to obtain laser data, and use an IMU device to obtain environmental data of the indoor space to obtain IMU data; the laser data and the IMU data are processed in time and space synchronization to obtain target laser data and target IMU data.

[0117] The gravity direction determination module 220 is used to extract the data of the IMU device at a stationary moment from the target IMU data, and determine the gravity direction in the device coordinate system corresponding to the IMU device based on the extracted data.

[0118] The dedistortion module 230 is used to determine the posture data of the IMU device for each frame of target laser data in the target laser data using the target IMU data with the same time as the current frame of target laser data, determine the posture data of the laser radar according to the posture data of the IMU device, interpolate the posture data of the laser radar, convert the current frame of target laser data to the global coordinate system to obtain dedistorted laser data, convert the dedistorted laser data to the posture data of the laser radar at the end of the current frame to obtain target dedistorted laser data; extract feature points of the target dedistorted laser data; the feature points include feature line points and feature surface points.

[0119] The initial map construction module 240 is used to use the first frame of target dedistorted laser data to construct an initial map point cloud through the feature line points and feature surface points of the first frame of target dedistorted laser data.

[0120] The map updating module 250 is used to determine the normal vectors of each feature point corresponding to the current frame target dedistorted laser data for each frame target dedistorted laser data after the first frame target dedistorted laser data, and cluster the normal vectors parallel to the gravity direction in the normal vectors to determine a set of candidate normal vectors; determine the valid feature points and the weights of each valid feature point according to the normal vectors of each feature point corresponding to the current frame target dedistorted laser data, the distances between each feature point and the center of the laser radar, and the set of candidate normal vectors; construct the line feature residual equation and the surface feature residual equation corresponding to the current frame target dedistorted laser data respectively using the weights and normal vectors of each feature point in the valid feature points; and determine the optimized position and optimized posture of each valid feature point using each line feature residual equation and each surface feature residual equation; perform validity check on the optimized position and optimized posture of each valid feature point; after the validity check passes, add each valid feature point to the initial map point cloud according to the optimized position and optimized posture of each valid feature point.

[0121] According to an embodiment of the present disclosure, the present disclosure also provides an electronic device and a computer-readable storage medium.

[0122] Figure 3 A schematic block diagram of an example electronic device 300 that can be used to implement an embodiment of the present disclosure is shown. The electronic device is intended to represent various forms of digital computers, such as laptop computers, desktop computers, workstations, personal digital assistants, servers, blade servers, mainframe computers, and other suitable computers. The electronic device can also represent various forms of mobile devices, such as personal digital processing, cellular phones, smart phones, wearable devices, and other similar computing devices. The components shown herein, their connections and relationships, and their functions are merely examples and are not intended to limit the implementation of the present disclosure described and / or required herein.

[0123] like Figure 3 As shown, the device 300 includes a computing unit 310, which can perform various appropriate actions and processes according to a computer program stored in a read-only memory (ROM) 320 or a computer program loaded from a storage unit 380 into a random access memory (RAM) 330. In the RAM 330, various programs and data required for the operation of the device 300 can also be stored. The computing unit 310, the ROM 320, and the RAM 330 are connected to each other via a bus 340. An input / output (I / O) interface 350 is also connected to the bus 340.

[0124] A number of components in the device 300 are connected to the I / O interface 350, including: an input unit 360, such as a keyboard, a mouse, etc.; an output unit 370, such as various types of displays, speakers, etc.; a storage unit 380, such as a disk, an optical disk, etc.; and a communication unit 390, such as a network card, a modem, a wireless communication transceiver, etc. The communication unit 390 allows the device 300 to exchange information / data with other devices through a computer network such as the Internet and / or various telecommunication networks.

[0125] The computing unit 310 may be a variety of general and / or special processing components with processing and computing capabilities. Some examples of the computing unit 310 include, but are not limited to, a central processing unit (CPU), a graphics processing unit (GPU), various dedicated artificial intelligence (AI) computing chips, various computing units running machine learning model algorithms, digital signal processors (DSPs), and any appropriate processors, controllers, microcontrollers, etc. The computing unit 310 performs the various methods and processes described above. For example, in some embodiments, any of the above methods may be implemented as a computer software program, which is tangibly contained in a machine-readable medium, such as a storage unit 380. In some embodiments, part or all of the computer program may be loaded and / or installed on the device 300 via the ROM 320 and / or the communication unit 390. When the computer program is loaded into the RAM 330 and executed by the computing unit 310, one or more steps of any of the methods described above may be performed. Alternatively, in other embodiments, the computing unit 310 may be configured to perform any of the methods described above in any other appropriate manner (e.g., by means of firmware).

[0126] Various implementations of the systems and techniques described above herein can be implemented in digital electronic circuit systems, integrated circuit systems, field programmable gate arrays (FPGAs), application specific integrated circuits (ASICs), application specific standard products (ASSPs), systems on chips (SOCs), load programmable logic devices (CPLDs), computer hardware, firmware, software, and / or combinations thereof. These various implementations can include: being implemented in one or more computer programs that can be executed and / or interpreted on a programmable system including at least one programmable processor, which can be a special purpose or general purpose programmable processor that can receive data and instructions from a storage system, at least one input device, and at least one output device, and transmit data and instructions to the storage system, the at least one input device, and the at least one output device.

[0127] The program code for implementing the method of the present disclosure may be written in any combination of one or more programming languages. These program codes may be provided to a processor or controller of a general-purpose computer, a special-purpose computer, or other programmable data processing device, so that the program code, when executed by the processor or controller, enables the functions / operations specified in the flow chart and / or block diagram to be implemented. The program code may be executed entirely on the machine, partially on the machine, partially on the machine and partially on a remote machine as a stand-alone software package, or entirely on a remote machine or server.

[0128] In the context of the present disclosure, a machine-readable medium may be a tangible medium that may contain or store a program for use by or in conjunction with an instruction execution system, device, or equipment. A machine-readable medium may be a machine-readable signal medium or a machine-readable storage medium. A machine-readable medium may include, but is not limited to, an electronic, magnetic, optical, electromagnetic, infrared, or semiconductor system, device, or equipment, or any suitable combination of the foregoing. A more specific example of a machine-readable storage medium may include an electrical connection based on one or more lines, a portable computer disk, a hard disk, a random access memory (RAM), a read-only memory (ROM), an erasable programmable read-only memory (EPROM or flash memory), an optical fiber, a portable compact disk read-only memory (CD-ROM), an optical storage device, a magnetic storage device, or any suitable combination of the foregoing.

[0129] To provide interaction with a user, the systems and techniques described herein can be implemented on a computer having: a display device (e.g., a CRT (cathode ray tube) or LCD (liquid crystal display) monitor) for displaying information to the user; and a keyboard and pointing device (e.g., a mouse or trackball) through which the user can provide input to the computer. Other types of devices can also be used to provide interaction with the user; for example, the feedback provided to the user can be any form of sensory feedback (e.g., visual feedback, auditory feedback, or tactile feedback); and input from the user can be received in any form (including acoustic input, voice input, or tactile input).

[0130] The systems and techniques described herein may be implemented in a computing system that includes back-end components (e.g., as a data server), or a computing system that includes middleware components (e.g., an application server), or a computing system that includes front-end components (e.g., a user computer with a graphical user interface or a web browser through which a user can interact with implementations of the systems and techniques described herein), or a computing system that includes any combination of such back-end components, middleware components, or front-end components. The components of the system may be interconnected by any form or medium of digital data communication (e.g., a communication network). Examples of communication networks include: a local area network (LAN), a wide area network (WAN), and the Internet.

[0131] A computer system may include a client and a server. The client and the server are generally remote from each other and usually interact through a communication network. The relationship of client and server is generated by computer programs running on respective computers and having a client-server relationship with each other. The server may be a cloud server, a server of a distributed system, or a server combined with a blockchain.

[0132] It should be understood that the various forms of processes shown above can be used to reorder, add or delete steps. For example, the steps recorded in this disclosure can be executed in parallel, sequentially or in different orders, as long as the desired results of the technical solutions disclosed in this disclosure can be achieved, and this document does not limit this.

[0133] The above specific implementations do not constitute a limitation on the protection scope of the present disclosure. It should be understood by those skilled in the art that various modifications, combinations, sub-combinations and substitutions can be made according to design requirements and other factors. Any modification, equivalent substitution and improvement made within the spirit and principle of the present disclosure shall be included in the protection scope of the present disclosure.

Claims

1. A method for constructing an indoor three-dimensional environment map based on laser SLAM, characterized in that: include: Scanning data of the indoor space using a laser radar to obtain laser data, and acquiring environmental data of the indoor space using an IMU device to obtain IMU data; Performing spatiotemporal synchronization processing on the laser data and the IMU data to obtain target laser data and target IMU data; Extracting data of the IMU device at rest from the target IMU data, and determining the gravity direction in the device coordinate system corresponding to the IMU device based on the extracted data; For each frame of target laser data in the target laser data, the target IMU data with the same time as the current frame of target laser data is used to determine the posture data of the IMU device, the posture data of the laser radar is determined according to the posture data of the IMU device, the posture data of the laser radar is interpolated, and the current frame of target laser data is converted to the global coordinate system to obtain dedistorted laser data, and the dedistorted laser data is converted to the posture data of the laser radar at the end of the current frame to obtain target dedistorted laser data; feature points of the target dedistorted laser data are extracted; the feature points include feature line points and feature surface points; Using the first frame of target dedistorted laser data, an initial map point cloud is constructed through the feature line points and feature surface points of the first frame of target dedistorted laser data; For each frame of target dedistorted laser data after the first frame of target dedistorted laser data, determine the normal vectors of each feature point corresponding to the current frame of target dedistorted laser data, and cluster the normal vectors parallel to the gravity direction in the normal vectors to determine a set of candidate normal vectors; determine valid feature points and the weight of each valid feature point according to the normal vectors of each feature point corresponding to the current frame of target dedistorted laser data, the distance between each feature point and the center of the laser radar, and the set of candidate normal vectors; and construct the line feature residual equation and the surface feature residual equation corresponding to the current frame of target dedistorted laser data using the weight and normal vector of each feature point in the valid feature points; Using the residual equations of each line feature and each surface feature, the optimal position and posture of each effective feature point are determined; Check the effectiveness of the optimized position and optimized posture of each valid feature point; After the validity check is passed, each valid feature point is added to the initial map point cloud according to the optimized position and optimized posture of each valid feature point.

2. The method according to claim 1, characterized in that The posture data of the IMU device and the posture data of the laser radar have the same frequency; The interpolation processing of the attitude data of the laser radar includes: The timestamp of each laser point in the target laser data of the current frame is interpolated, and the attitude data corresponding to each interpolated timestamp is determined according to the attitude data of the laser radar.

3. The method according to claim 1, characterized in that The determining of the normal vectors of each feature point corresponding to the dedistorted laser data of the target in the current frame includes: For each feature point corresponding to the dedistorted laser data of the target in the current frame, the adjacent points of the feature point are searched in the initial map point cloud, and the normal vector of the feature point is determined using the searched adjacent points.

4. The method according to claim 1, characterized in that The determining of the valid feature points and the weight of each valid feature point according to the normal vector of each feature point corresponding to the dedistorted laser data of the current frame target, the distance between each feature point and the center of the laser radar, and the set of candidate normal vectors includes: For each feature point corresponding to the dedistorted laser data of the target in the current frame, if the normal vector of the feature point does not belong to the set of candidate normal vectors, the distance between the feature point and the center of the laser radar is less than the first preset distance, and it does not belong to the vector perpendicular to the direction of gravity, then the feature point is determined to be an invalid feature point and the feature point is eliminated; if the normal vector of the feature point does not belong to the set of candidate normal vectors, and the distance between the feature point and the center of the laser radar is less than the second preset distance, then the feature point is determined to be a valid feature point, and a weight value within a first numerical range is set for the feature point; if the distance between the feature point and the center of the laser radar is greater than the second preset distance, then the feature point is determined to be a valid feature point, and a weight value within a second numerical range is set for the feature point; wherein the weight value within the first numerical range is less than the weight value within the second numerical range.

5. The method according to claim 1, characterized in that It also includes using the weights and normal vectors of each feature line point in the effective feature points to construct the line feature residual equation corresponding to each frame of target dedistorted laser data: The line feature residual equation corresponding to each frame of target dedistorted laser data is constructed using the following formulas: Wherein, i represents the data frame after the first frame of target de-distorted laser data, m represents the value obtained by minus 1 of the frame number of target de-distorted laser data; k represents the feature line extracted from one frame of target de-distorted laser data, h represents the number of feature lines extracted from one frame of target de-distorted laser data; j represents the feature line point on the feature line, and n represents the number of feature line points on one feature line; L k is the extracted line parameter, R i is the pose of the dedistorted laser data of the i-th frame target to be optimized, P j is the line feature of the jth feature line point, W j is the weight value corresponding to the current feature line point.

6. The method according to claim 1, characterized in that It also includes using the weights and normal vectors of each feature surface point in the effective feature points to construct the surface feature residual equation corresponding to each frame of target dedistorted laser data: The surface feature residual equation corresponding to each frame of target dedistorted laser data is constructed using the following formulas: Wherein, i represents the data frame after the first frame of target dedistortion laser data, m represents the value obtained by minus 1 of the frame number of target dedistortion laser data; l represents the feature surface extracted from one frame of target dedistortion laser data, f represents the number of feature surfaces extracted from one frame of target dedistortion laser data; b represents the feature surface points on the feature surface, and v represents the number of feature surface points on one feature surface; N l is the extracted plane parameter, R i is the pose of the dedistorted laser data of the i-th frame target to be optimized, P b is the surface feature of the extracted feature points, W b is the weight value corresponding to the current feature point.

7. The method according to claim 1, characterized in that The method of using each line feature residual equation and each surface feature residual equation to determine the optimal position and optimal posture of each effective feature point includes: Through the nonlinear optimization algorithm, the residual equations of each line feature and each surface feature residual equation are processed to obtain the optimized position and optimized posture of each effective feature point.

8. The method according to claim 1, characterized in that The validity check of the optimized position and optimized posture of each valid feature point includes: The optimized position of each valid feature point is used to determine the optimized residual value. If the difference between the optimized residual value and the residual value before optimization determined by the feature points before optimization meets the preset requirements and the number of valid feature points is greater than the preset number of points, the validity check is determined to have passed.

9. The method according to claim 1, characterized in that: After the validity check is passed, the following is also included: The normal vector of each valid feature point in the initial map point cloud is calculated respectively.

10. A device for constructing indoor three-dimensional environment maps based on laser SLAM, characterized in that: include: A data acquisition synchronization module is used to use a laser radar to scan the indoor space to obtain laser data, and use an IMU device to obtain environmental data of the indoor space to obtain IMU data; the laser data and the IMU data are processed in time and space synchronization to obtain target laser data and target IMU data; A gravity direction determination module, used to extract data of the IMU device at a stationary moment from the target IMU data, and determine the gravity direction in the device coordinate system corresponding to the IMU device based on the extracted data; A dedistortion module is used to determine the posture data of the IMU device for each frame of target laser data in the target laser data by using the target IMU data with the same time as the current frame of target laser data, determine the posture data of the laser radar according to the posture data of the IMU device, perform interpolation processing on the posture data of the laser radar, and convert the current frame of target laser data to the global coordinate system to obtain dedistorted laser data, convert the dedistorted laser data to the posture data of the laser radar at the end of the current frame to obtain target dedistorted laser data; extract feature points of the target dedistorted laser data; the feature points include feature line points and feature surface points; An initial map construction module is used to use the first frame of target dedistorted laser data to construct an initial map point cloud through feature line points and feature surface points of the first frame of target dedistorted laser data; A map updating module is used to determine the normal vectors of each feature point corresponding to the current frame target dedistorted laser data for each frame target dedistorted laser data after the first frame target dedistorted laser data, and cluster the normal vectors parallel to the gravity direction in the normal vectors to determine a set of candidate normal vectors; determine valid feature points and the weight of each valid feature point according to the normal vectors of each feature point corresponding to the current frame target dedistorted laser data, the distance between each feature point and the center of the laser radar, and the set of candidate normal vectors; and construct the line feature residual equation and the surface feature residual equation corresponding to the current frame target dedistorted laser data using the weight and normal vector of each feature point in the valid feature points; And, using each line feature residual equation and each surface feature residual equation, determine the optimal position and optimal pose of each effective feature point; Check the effectiveness of the optimized position and optimized posture of each valid feature point; After the validity check is passed, each valid feature point is added to the initial map point cloud according to the optimized position and optimized posture of each valid feature point.

Images