A lidar-based relocalization method suitable for indoor environments

Abstract

The application discloses a kind of indoor environment's laser radar-based repositioning method, solve the technical problem of low degree of recognition of traditional Lidar repositioning method point cloud data, point cloud matching error is larger, repositioning precision is lower.The application is a kind of laser radar-based repositioning method, by accumulating the laser radar point cloud of a certain time, and generates repositioning point cloud descriptor, then accurately matches the point cloud of same position in historical map, calculates the pose of repositioning point cloud relative to historical map.The application enables the mobile device equipped with laser radar to realize fast and accurate positioning in the point cloud map under the premise of using only laser radar sensor information, and not relying on any prior information.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of laser radar repositioning, and particularly relates to a laser radar-based repositioning method suitable for indoor environments. BACKGROUND

[0002] Lidar-based repositioning algorithms use Lidar-collected environmental point cloud data 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 light and weather, making them particularly suitable for low-light or dynamic scenarios. This technology is widely used in autonomous driving, unmanned aerial vehicle navigation, robot path planning, and other fields, meeting the demand for high-precision, high-robustness position perception, especially in environments where GPS signals are weak or fail (such as tunnels, forests, etc.), providing reliable map matching and positioning support for autonomous systems.

[0003] Existing pure Lidar-based repositioning algorithms mainly include point cloud matching, feature extraction and description, and multi-resolution map methods. These methods require prior pose information as an initial value to successfully match and reposition. In indoor environments, there are dynamic objects such as pedestrians and moving furniture, and the point cloud data has low resolution, which can easily lead to point cloud matching errors and affect repositioning accuracy. SUMMARY

[0004] The purpose of the present application is to provide a Lidar-based repositioning method suitable for indoor environments, which solves the technical problem of low resolution of point cloud data, large point cloud matching errors, and low repositioning accuracy in traditional Lidar repositioning methods.

[0005] To achieve the above purpose, the technical solution adopted by the present application is as follows:

[0006] A Lidar-based repositioning method suitable for indoor environments includes the following steps:

[0007] S1. Obtain a single frame of point cloud data using a Lidar, convert the single frame of point cloud data to a unified coordinate system using known pose information, and accumulate multiple frames of point cloud data in the unified coordinate system to form a sub-map;

[0008] S2. Using the method of spatial voxels, divide the laser point cloud of the sub-map into different spatial voxels according to the coordinates of the points, and form a voxel map according to the preset resolution of the spatial voxels;

[0009] S3. Perform plane fitting in each spatial voxel and extract planes that meet the conditions;

[0010] S4. Merge coplanar planes into large planes to form a large-scale geometric description of the sub-map;

[0011] 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;

[0012] 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;

[0013] 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.

[0014] 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.

[0015] 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.

[0016] Furthermore, in S3, the plane fitting adopts the PCA method to perform plane fitting within spatial voxels where the number of points exceeds a preset threshold.

[0017] 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.

[0018] 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.

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

[0020] 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.

[0021] 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.

[0022] 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.

[0023] Compared with the prior art, the present invention has the following beneficial effects:

[0024] 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

[0025] Figure 1 This is a flowchart of the method of the present invention. DETAILED DESCRIPTION

[0026] TERMS EXPLANATION

[0027] The PCA method is a principal component analysis method.

[0028] The SVD decomposition method is a matrix decomposition method.

[0029] The GICP method is a point cloud registration method.

[0030] In order to make the objects, technical solutions and advantages of the present application clearer, further detailed description will be made to the present application in combination with the drawings. Obviously, the described embodiments are only some of the embodiments of the present application, but not all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor fall within the protection scope of the present application.

[0031] As shown in Figure 1 , the present application provides a laser radar-based repositioning method suitable for indoor environment, comprising the following steps:

[0032] S1, acquiring a single-frame point cloud by a laser radar, converting the single-frame point cloud to a unified coordinate system by known pose information, and accumulating multiple frames of point clouds in the unified coordinate system to form a submap;

[0033] S2, dividing the laser point cloud of the submap into different space voxels according to the coordinates of the points by a space voxel method according to a preset space voxel resolution, to form a voxel map;

[0034] S3, performing plane fitting in each space voxel, and extracting planes meeting the conditions;

[0035] S4, merging coplanar planes into large planes to form a large-scale geometric description of the submap;

[0036] S5, selecting the first l large planes with the largest number of merged planes as the description planes of the current submap;

[0037] S6, importing the description planes of each historical submap in the repositioning map, and calculating the pose transformation between the current submap and each historical submap;

[0038] S7, based on the pose transformation, converting the description planes of the current submap to the coordinate system of each historical submap, and calculating the center point distance error and the normal vector angle error for evaluating the matching error of two submaps;

[0039] S8, selecting the historical submap with the smallest matching error as the optimal matching submap, and recording the pose transformation between the current submap and the optimal matching submap as the initial pose transformation;

[0040] 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.

[0041] 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.

[0042] 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.

[0043] 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... i.e. the plane normal vector, the coordinate mean value i.e. the plane center point.

[0044] In some embodiments of the present application, S5 takes the plane center distance less than a set distance threshold and the included angle of the plane normal vector less than a set angle threshold as the coplanar condition, records the planes satisfying the coplanar condition as coplanar planes and combines them into one large plane, and records the number of planes contained in the large plane after combination.

[0045] All the large planes after combination are sorted according to the number of small planes they contain from large to small to obtain the first l large planes with the largest number of small planes and take them as the current submap submap t . l Each description plane includes a normal vector and a center, ). Then, the description planes (including normal vectors N and center points submap ) of the first submap history submaps N imported in the repositioning map (including normal vectors and center points , the history submaps are the first N submaps obtained, each history submap also includes l description planes, and corresponds to the first submap description planes in the submap t . For the current submap l t and each history submap submap i in the repositioning map, a matching relationship between the description planes of the two submaps is constructed, submap , , , i is the number of the corresponding history submap. Then, the pose transformation between the current submap and each history submap is calculated, such as the coordinate system transformation relationship submap t between the current submap submap i and the history submap .

[0046] In some embodiments, in S6, the pose transformation between the current submap and each history submap is calculated by the SVD decomposition method.

[0047] According to the current submap submap t and the history submap submapi Coordinate system transformation relationship between It can display the current submap submap t The center point of all descriptive planes and normal vector All have been converted to the historical submap submap i In the coordinate system, we obtain and This allows us to calculate the distance error between the center points of the matching description planes and the angle error 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 submap t 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 submap 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.

[0048] Select the historical sub-map with the smallest matching error submap o As the optimal matching submap, the pose transformation between the current submap and the optimal matching submap is... This is denoted as the initial pose transformation. The repositioned pose is calculated using the GICP method, specifically based on the current sub-map. submap t Point cloud and optimal matching submap submap o The point cloud is used as input and the initial pose transformation Using the initial value, 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.

[0049] Finally, it should be noted that: the above embodiments are merely the preferred embodiments of the present application to illustrate the technical solutions of the present application, rather than limit, of course, is not to limit the scope of the patent of the present application; although the present application is described in detail with reference to the foregoing embodiments, those skilled in the art should understand that: it can still be modified to the technical solutions recorded in the foregoing embodiments, or part or all of the technical features are replaced; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the scope of the technical solutions of the embodiments of the present application; that is to say, but whatever is made in the main design idea and spirit of the present application has no substantial significance of the change or polish, the technical problem solved is still consistent with the present application, and should be included in the protection scope of the present application; in addition, the technical solutions of the present application are directly or indirectly applied to other related technical fields, which are also included in the patent protection scope of the present application.

Claims

1. A lidar-based relocalization method suitable for indoor environments, characterized in that, The method comprises the following steps: S1, obtaining a single-frame point cloud by a laser radar, converting the single-frame point cloud to a unified coordinate system by known pose information, accumulating multiple frames of point clouds in the unified coordinate system to form a submap; S2, dividing laser point clouds of the submap into different spatial voxels according to the coordinates of the points by a spatial voxel method according to a preset spatial voxel resolution to form a voxel map; S3, performing plane fitting in each spatial voxel to extract planes meeting the conditions; S4, merging coplanar planes into a large plane to form a large-scale geometric description of the submap; S5, select the first several largest planes as the description planes of the current submap; l S5, select the first several largest planes as the description planes of the current submap; S6, importing description planes of each historical submap in the repositioning map, and calculating pose transformations between the current submap and each historical submap; S7, based on the pose transformations, converting the description planes of the current submap to the coordinate systems of each historical submap, and calculating central point distance errors and normal vector angle errors for evaluating matching errors of two submaps; S8, selecting a historical submap with the minimum matching error as an optimal matching submap, and recording a pose transformation between the current submap and the optimal matching submap as an initial pose transformation; S9, taking point clouds of the current submap and the optimal matching submap as inputs and the initial pose transformation as an initial value to obtain an optimal pose transformation between the current submap and the optimal matching submap, i.e., a repositioning pose.

2. The lidar-based relocalization method for indoor environments of claim 1, wherein, In S3, the plane fitting adopts a PCA method and is performed in each spatial voxel with a number of points exceeding a preset threshold.

3. The lidar-based relocalization method for indoor environments of claim 1, wherein, S3 The method comprises the following steps: S31, calculating the mean value of the coordinates of all points in a spatial voxel , wherein is the coordinate of the i i th point, N is the number of points; S32, calculating the covariance of the mean value of the coordinates ; S33, performing eigenvalue decomposition on the covariance to obtain the minimum eigenvalue of the covariance and the corresponding eigenvector ; S34, if the minimum eigenvalue is less than a preset threshold value, it represents that the points of the body in the spatial voxel are on a plane, and the corresponding eigenvector is the normal vector of the plane, and the mean value of the coordinates is the center point of the plane.

4. The lidar-based relocalization method for indoor environments of claim 1, wherein, In S5, a coplanar condition is that a plane center distance is less than a set distance threshold and a plane normal vector angle is less than a set angle threshold, planes meeting the coplanar condition are recorded as coplanar planes and merged into a large plane, and the number of planes included in the large plane after merging is recorded.

5. The lidar-based relocalization method for indoor environments of claim 1, wherein, In S6, the SVD decomposition method is used to calculate the pose transformations between the current submap and each historical submap.

6. The lidar-based relocalization method for indoor environments of claim 1, wherein, 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 formulas and , the center point distance error and the normal vector angle error between the current submap and the corresponding historical submap are obtained and , respectively, are the center point and normal vector of the corresponding description plane in the historical submap i . and .

7. The lidar-based relocalization method for indoor environments of claim 1, wherein, In S7, the smaller the central point distance error and the normal vector angle error are, the higher the matching degree of two submaps is.

8. The lidar-based relocalization method for indoor environments of claim 1, wherein, In S9, the point clouds of the current submap and the optimal matching submap are taken as inputs and the initial pose transformation is taken as an initial value to obtain an optimal pose transformation between the current submap and the optimal matching submap.

Images