Laser radar-based repositioning method suitable for indoor environment

Abstract

The invention discloses a laser radar-based repositioning method suitable for an indoor environment, and solves the technical problems of low identification degree of point cloud data, large point cloud matching error and low repositioning precision of a traditional Lidar repositioning method. According to the relocation method based on the laser radar, laser radar point clouds in a certain time are accumulated, relocation point cloud descriptors are generated, point clouds at the same positions are accurately matched in a historical map, and the poses of the relocation point clouds relative to the historical map are obtained through calculation. According to the invention, the mobile device carrying the laser radar can realize rapid and accurate positioning in the point cloud map on the premise of only using the information of the laser radar sensor and not depending on any other prior information.

Description

Technical Field

[0001] This invention relates to the field of lidar relocation technology, and more specifically to a lidar-based relocation method suitable for indoor environments. Background Technology

[0002] LiDAR-based relocalization algorithms utilize environmental point cloud data collected by LiDAR to achieve high-precision positioning of vehicles or robots in complex environments. Compared to visual data, LiDAR point clouds are more robust to changes in lighting and weather, making them particularly suitable for low-light or dynamic scenarios. This technology is widely used in autonomous driving, drone navigation, and robot path planning, meeting the requirements for high-precision and robust positional awareness, especially in environments with weak or unavailable GPS signals (such as tunnels and forests), providing reliable map matching and positioning support for autonomous systems.

[0003] Existing LiDAR-based relocalization algorithms mainly include point cloud matching, feature extraction and description, and multi-resolution mapping. These methods all require prior pose information as initial values ​​to successfully match and relocalize. In indoor environments, dynamic objects such as pedestrians and moving furniture exist, and the low resolution of point cloud data can easily lead to point cloud matching errors, affecting relocalization accuracy. Summary of the Invention

[0004] The purpose of this invention is to provide a LiDAR-based relocation method suitable for indoor environments, which solves the technical problems of low point cloud data recognition, large point cloud matching error, and low relocation accuracy in traditional LiDAR relocation methods.

[0005] To achieve the above objectives, the technical solution adopted by the present invention is as follows: A lidar-based relocation method suitable for indoor environments includes the following steps: S1. Acquire single-frame point clouds using lidar, transform the single-frame point clouds to a unified coordinate system using known pose information, and accumulate multiple frames of point clouds in the unified coordinate system to form a sub-map. S2. Using the spatial voxel method, according to the preset spatial voxel resolution, the laser point cloud of the sub-map is divided into different spatial voxels according to the coordinates of the points to form a voxel map. S3. Perform plane fitting within each spatial voxel and extract planes that meet the conditions; S4. Merge coplanar planes into a larger plane to form a large-scale geometric description of the sub-map; S5. Select the plane with the largest number of merged planes. l A large plane serves as the description plane for the current sub-map; S6. Import the description planes of each historical sub-map in the relocation map, and calculate the pose transformation between the current sub-map and each historical sub-map; S7. Based on pose transformation, transform the description plane of the current sub-map to the coordinate system of each historical sub-map, and calculate the center point distance error and the angle error between the normal vectors used to evaluate the matching error between the two sub-maps. S8. Select the historical sub-map with the smallest matching error as the optimal matching sub-map, and record the pose transformation between the current sub-map and the optimal matching sub-map as the initial pose transformation. S9. Using the point cloud of the current sub-map and the point cloud of the best matching sub-map as input and the initial pose transformation as the initial value, obtain the optimal pose transformation between the current sub-map and the best matching sub-map, i.e. the pose of relocalization.

[0006] Furthermore, in S3, the PCA method is used for plane fitting, and plane fitting is performed within spatial voxels where the number of points exceeds a preset threshold.

[0007] Furthermore, S3 includes: S31, calculating the mean coordinates of all points contained within the spatial voxel. ,in For the first i The coordinates of the points N The number of points; S32, through the coordinate mean Calculate covariance S33, regarding covariance Perform eigenvalue decomposition to obtain the smallest eigenvalue of the covariance. and the corresponding feature vector S34. If the smallest eigenvalue Points smaller than a preset threshold indicate that the points within a spatial voxel lie on a plane, and the corresponding feature vectors... That is, the plane normal vector, the mean of the coordinates. That is, the center point of the plane.

[0008] Furthermore, in S5, planes whose center distance is less than a set distance threshold and whose angle between plane normal vectors is less than a set angle threshold are used as coplanar conditions. Planes that meet the coplanar conditions are recorded as coplanar planes and merged into a large plane. The number of planes contained in the merged large plane is recorded.

[0009] Furthermore, in S6, the pose transformation between the current submap and each historical submap is calculated using the SVD decomposition method.

[0010] Furthermore, in S7, through pose transformation between the current submap and each historical submap, the center points and normal vectors of all description planes in the current submap are transformed to the corresponding historical submap coordinate system, thus obtaining the corresponding transformation center points. and transformation normal vector ; and These are the first and second sub-maps in the current sub-map. m The transformation center point and transformation normal vector of each description plane are calculated using the coordinate system of the corresponding historical sub-map. l To describe the number of planes, i The numbering of historical sub-maps; according to the formula and This yields the distance error between the center points of the current submap and the corresponding historical submap. Error between the angle and the normal vector , and In the historical sub-map i Zhongyu and The corresponding description plane's center point and normal vector.

[0011] Furthermore, in S7, the smaller the error between the center point distance error and the angle error between the normal vectors, the higher the degree of matching between the two sub-maps.

[0012] Furthermore, in S9, the optimal pose transformation between the current sub-map and the optimal matching sub-map is obtained by using the point cloud of the current sub-map and the point cloud of the optimal matching sub-map as input and the initial pose transformation as initial value.

[0013] Compared with the prior art, the present invention has the following beneficial effects: This invention presents a LiDAR-based relocalization method for indoor scenes. It accumulates LiDAR point clouds over a certain period, generates a relocalization point cloud descriptor, and then accurately matches point clouds at the same location in a historical map to calculate the pose of the relocalized point cloud relative to the historical map. This invention solves the problem of relocalization in indoor environments without prior pose input using LiDAR, enabling LiDAR-equipped mobile devices to quickly and accurately locate themselves within an existing point cloud map using only LiDAR sensor information and without relying on any other prior information. Attached Figure Description

[0014] Figure 1 This is a flowchart of the method of the present invention. Detailed Implementation

[0015] Terminology Explanation: PCA is a principal component analysis method.

[0016] SVD decomposition is a matrix decomposition method.

[0017] GICP is a point cloud registration method.

[0018] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings. Obviously, the described embodiments are merely some embodiments of this invention, and not all embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of this invention without creative effort are within the scope of protection of this invention.

[0019] like Figure 1 As shown, the present invention provides a lidar-based relocation method suitable for indoor environments, comprising the following steps: S1. Acquire single-frame point clouds using lidar, transform the single-frame point clouds to a unified coordinate system using known pose information, and accumulate multiple frames of point clouds in the unified coordinate system to form a sub-map. S2. Using the spatial voxel method, according to the preset spatial voxel resolution, the laser point cloud of the sub-map is divided into different spatial voxels according to the coordinates of the points to form a voxel map. S3. Perform plane fitting within each spatial voxel and extract planes that meet the conditions; S4. Merge coplanar planes into a larger plane to form a large-scale geometric description of the sub-map; S5. Select the plane with the largest number of merged planes. l A large plane serves as the description plane for the current sub-map; S6. Import the description planes of each historical sub-map in the relocation map, and calculate the pose transformation between the current sub-map and each historical sub-map; S7. Based on pose transformation, transform the description plane of the current sub-map to the coordinate system of each historical sub-map, and calculate the center point distance error and the angle error between the normal vectors used to evaluate the matching error between the two sub-maps. S8. Select the historical sub-map with the smallest matching error as the optimal matching sub-map, and record the pose transformation between the current sub-map and the optimal matching sub-map as the initial pose transformation. S9. Using the point cloud of the current sub-map and the point cloud of the best matching sub-map as input and the initial pose transformation as the initial value, obtain the optimal pose transformation between the current sub-map and the best matching sub-map, i.e. the pose of relocalization.

[0020] This invention accumulates single-frame laser point clouds acquired from LiDAR, forming a point cloud sub-map when the accumulation reaches a certain amount. Using a voxel mapping method, the sub-map laser point cloud is divided into spatial voxels at a certain voxel size, resulting in a voxel map of the sub-map point cloud. Plane fitting is performed within each spatial voxel to obtain all planes that meet the specified conditions. All fitted planes are merged, combining smaller planes into a larger, complete plane, resulting in a large-scale geometric description of the scene. The largest merged planes are selected as the description planes for the current location. The description planes of all historical sub-maps in the relocation map are imported, and based on the description planes of the current sub-map and each historical sub-map, the two sub-maps are solved. The pose transformation between the maps is calculated. Based on the solved pose transformation, the description plane of the current sub-map is transformed to the coordinate system of the corresponding historical sub-map. The center point distance error and the angle error between the normal vectors of the two sub-maps are calculated as indicators of the matching degree between the two sub-maps. Based on the matching degree indicators, the historical sub-map with the smallest center point distance error and the smallest angle error between the normal vectors is selected as the optimal matching sub-map. Using the point cloud of the current sub-map and the point cloud of the optimal matching sub-map as input, and the pose transformation of the optimal matching sub-map as the initial value, the pose transformation between the sub-maps is further optimized more accurately to obtain the optimal pose transformation between the current sub-map and the optimal matching sub-map, i.e., the relocalized pose.

[0021] In some embodiments of the present invention, in step S3, the plane fitting employs the PCA method, performing plane fitting within spatial voxels where the number of points exceeds a preset threshold. The preset threshold is set to 5, but in practice, an optimal threshold needs to be calculated based on specific circumstances.

[0022] In some embodiments of the present invention, S3 includes: S31, calculating the mean coordinates of all points contained within a spatial voxel. ,in For the first i The coordinates of the points N The number of points; S32, through the coordinate mean Calculate covariance S33, regarding covariance Perform eigenvalue decomposition to obtain the smallest eigenvalue of the covariance. and the corresponding feature vector S34. If the smallest eigenvalue Points smaller than a preset threshold indicate that the points within a spatial voxel lie on a plane, and the corresponding feature vectors... That is, the plane normal vector, the mean of the coordinates. That is, the center point of the plane.

[0023] In some embodiments of the present invention, S5 uses the fact that the distance between the centers of the planes is less than a set distance threshold and the angle between the plane normal vectors is less than a set angle threshold as coplanar conditions. The planes that meet the coplanar conditions are recorded as coplanar planes and merged into a large plane. The number of planes contained in the merged large plane is recorded.

[0024] Sort all the merged large planes in descending order according to the number of small planes they contain, and obtain the largest number of small planes. l A large plane is used as the current submap. subfolder t The description planes (each description plane includes a normal vector and a center) l Each describing plane includes normal vectors. and center point Then import it into the relocation map. N Historical sub-maps ( subfolder 1, ..., subfolder N The descriptive plane (including the normal vector) and center point The historical submap is the one obtained previously. N Each sub-map, and each historical sub-map also includes... l A descriptive plane, and with submaps subfolder t middle l Each describes a corresponding plane. For the current submap subfolder t and each historical submap in the relocation map subfolder i Construct a matching relationship between the two sub-maps to describe the plane. , , i This corresponds to the number of the historical submap. Then, calculate the pose transformation between the current submap and each historical submap, such as the current submap... subfolder t With historical submap subfolder i Coordinate system transformation relationship between .

[0025] In some embodiments, in S6, the pose transformation between the current sub-map and each historical sub-map is calculated using the SVD decomposition method.

[0026] Based on the current submap subfolder t With historical submap subfolder i Coordinate system transformation relationship between It can display the current submap subfoldert The center point of all descriptive planes and normal vector All have been converted to the historical submap subfolder i In the coordinate system, we obtain and This allows us to calculate the error in the distance between the center points of the matching description planes and the error in the angle between the normal vectors. Specifically, according to the formula... and This yields the distance error between the center points of the current submap and the corresponding historical submap. Error between the angle and the normal vector , and Each represents the current sub-map subfolder t The Middle m The transformation center point and transformation normal vector of each description plane are calculated using the coordinate system of the corresponding historical sub-map. and In the historical sub-map subfolder i Zhongyu and This corresponds to the center point and normal vector of the described plane. The smaller the error between the center point distance and the angle error between the normal vector, the higher the degree of matching between the two sub-maps.

[0027] Select the historical sub-map with the smallest matching error subfolder o As the optimal matching submap, the pose transformation between the current submap and the optimal matching submap. This is denoted as the initial pose transformation. The repositioned pose is calculated using the GICP method, specifically based on the current sub-map. subfolder t Point cloud and optimal matching submap subfolder o The point cloud is used as input and the initial pose transformation Using the initial values, call its registration interface. After successful registration optimization, the optimized pose transformation can be obtained through its interface, which is the optimal pose transformation between the current submap and the best-matching submap. That is, the pose for repositioning.

[0028] Finally, it should be noted that the above embodiments are merely preferred embodiments of the present invention used to illustrate the technical solutions of the present invention, and are not intended to limit the invention, nor are they intended to limit the patent scope of the present invention. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features therein. These modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention. That is to say, any changes or refinements made to the main design concept and spirit of the present invention that are not of substantial significance, but whose technical problems are still consistent with the present invention, should be included within the protection scope of the present invention. In addition, the direct or indirect application of the technical solutions of the present invention to other related technical fields are similarly included within the patent protection scope of the present invention.

Claims

1. A lidar-based relocation method suitable for indoor environments, characterized in that, Includes the following steps: S1. Acquire single-frame point clouds using lidar, transform the single-frame point clouds to a unified coordinate system using known pose information, and accumulate multiple frames of point clouds in the unified coordinate system to form a sub-map. S2. Using the spatial voxel method, according to the preset spatial voxel resolution, the laser point cloud of the sub-map is divided into different spatial voxels according to the coordinates of the points to form a voxel map. S3. Perform plane fitting within each spatial voxel and extract planes that meet the conditions; S4. Merge coplanar planes into a larger plane to form a large-scale geometric description of the sub-map; S5. Select the plane with the largest number of merged planes. l A large plane serves as the description plane for the current sub-map; S6. Import the description planes of each historical sub-map in the relocation map, and calculate the pose transformation between the current sub-map and each historical sub-map; S7. Based on pose transformation, transform the description plane of the current sub-map to the coordinate system of each historical sub-map, and calculate the center point distance error and the angle error between the normal vectors used to evaluate the matching error between the two sub-maps. S8. Select the historical sub-map with the smallest matching error as the optimal matching sub-map, and record the pose transformation between the current sub-map and the optimal matching sub-map as the initial pose transformation. S9. Using the point cloud of the current sub-map and the point cloud of the best matching sub-map as input and the initial pose transformation as the initial value, obtain the optimal pose transformation between the current sub-map and the best matching sub-map, i.e. the pose of relocalization.

2. The lidar-based relocation method suitable for indoor environments according to claim 1, characterized in that, In S3, the PCA method is used for plane fitting, which performs plane fitting within spatial voxels where the number of points exceeds a preset threshold.

3. The lidar-based relocation method suitable for indoor environments according to claim 1, characterized in that, S3 Includes: S31, Calculating the mean coordinates of all points contained within a spatial voxel. ,in For the first i The coordinates of the points N The number of points; S32, through the coordinate mean Calculate covariance S33, regarding covariance Perform eigenvalue decomposition to obtain the smallest eigenvalue of the covariance. and the corresponding feature vector S34. If the smallest eigenvalue Points smaller than a preset threshold indicate that the points within a spatial voxel lie on a plane, and the corresponding feature vectors... That is, the plane normal vector, the mean of the coordinates. That is, the center point of the plane.

4. The lidar-based relocation method suitable for indoor environments according to claim 1, characterized in that, In S5, planes whose center distance is less than a set distance threshold and whose angle between normal vectors is less than a set angle threshold are used as coplanar conditions. Planes that meet the coplanar conditions are recorded as coplanar planes and merged into a large plane. The number of planes contained in the merged large plane is recorded.

5. A lidar-based relocation method suitable for indoor environments according to claim 1, characterized in that, In S6, the pose transformation between the current submap and each historical submap is calculated using the SVD decomposition method.

6. A lidar-based relocation method suitable for indoor environments according to claim 1, characterized in that, In S7, through pose transformation between the current submap and each historical submap, the center points and normal vectors of all description planes in the current submap are transformed to the coordinate system of the corresponding historical submap, thus obtaining the corresponding transformation center points. and transformation normal vector ; and These are the first and second sub-maps in the current sub-map. m The transformation center point and transformation normal vector of each description plane are calculated using the coordinate system of the corresponding historical sub-map. l To describe the number of planes, i The number for the historical sub-map; According to the formula and This yields the distance error between the center points of the current submap and the corresponding historical submap. Error between the angle and the normal vector , and In the historical sub-map i Zhongyu and The corresponding description plane's center point and normal vector.

7. A lidar-based relocation method suitable for indoor environments according to claim 1, characterized in that, In S7, the smaller the error between the center point distance and the angle error between the normal vectors, the higher the degree of matching between the two sub-maps.

8. A lidar-based relocation method suitable for indoor environments according to claim 1, characterized in that, In S9, the optimal pose transformation between the current sub-map and the optimal matching sub-map is obtained by taking the point cloud of the current sub-map and the point cloud of the best matching sub-map as input and the initial pose transformation as initial value.

Images