Repositioning method, device and medium based on 3D laser radar and prior map

Abstract

The present invention discloses a repositioning method, device and medium based on 3D laser radar and prior map, including: performing point cloud preprocessing and point cloud segmentation on laser radar data and inertial measurement unit data to obtain a point cloud to be registered; establishing a multi-resolution pyramid grid map according to a 2.5D grid map; judging whether it is necessary to perform initial registration on the point cloud to be registered, if so, proceeding to step 4; otherwise, obtaining the initial repositioning result and proceeding to step 5; performing 2D coarse registration based on branch and bound search according to the pyramid grid map and the point cloud to be registered, performing 3D fine registration based on ICP registration according to the result of 2D coarse registration, the point cloud to be registered and the 3D point cloud map, completing the initial repositioning after iterative optimization convergence, and obtaining the initial repositioning result; performing incremental frame-image registration according to the initial repositioning result and the 3D point cloud map to obtain a continuous repositioning result. The present invention ensures robustness and improves calculation efficiency.

Description

Technical Field

[0001] The present invention relates to the field of navigation and positioning technology, and in particular to a repositioning method, device and medium based on 3D laser radar and a priori map. Background Art

[0002] The relocalization of a mobile robot can be divided into the initial relocalization phase and the continuous relocalization phase after the initial alignment. There are two main initial relocalization methods for robots based on LiDAR:

[0003] Branch and bound search methods based on 2D projection, such as the global registration method based on branch and bound proposed by Wolfgang Hess et al. for the first time, by establishing a multi-resolution pyramid map, calculating the score of overlapping pixels at a certain interval, and establishing a priority queue, using the depth-first search algorithm to iteratively calculate and obtain the optimal solution for the starting pose; Kenji Koide et al. proposed a positioning system HDL based on a priori maps, which expanded the above global registration method in 3D, selected point clouds within a specific height range and projected them to the plane, and then used the branch and bound method to search for the optimal solution; feature descriptor-based registration methods, such as the FPFH descriptor proposed by Resu et al., calculated the spatial geometric relationship between each point in the map and its neighboring points for encoding, increased the dimension of each point, and stored data by establishing a kd-Tree, and then performed the same encoding on the registered point cloud, and both performed nearest neighbor search to obtain their most likely positions in the map to complete the initial alignment. The global registration method based on branch and bound and its extended method ensure real-time performance, but lose multi-dimensional information, which can easily lead to mismatching in repetitive scenes; the method based on feature descriptors works well when the scene features are obvious, but it often fails when the scene features are sparse. At the same time, the calculation of feature descriptors is very time-consuming and cannot meet the speed requirements. Therefore, the above methods will cause mismatching or failure to converge in repetitive or sparse scenes, making it difficult to meet the initial alignment requirements in terms of accuracy and speed. In terms of 3D point cloud alignment, the 2D branch and bound method reduces the information dimension of the original point cloud, resulting in low robustness and low accuracy; the descriptor-based method is computationally intensive, time-consuming, and relies on complex environments. Summary of the invention

[0004] In view of this, the purpose of the present invention is to propose a relocalization method based on 3D lidar and prior map, a 2D+3D combined coarse-to-fine global initial alignment and continuous relocalization method, which improves the efficiency of calculation while ensuring robustness, and effectively ensures that the robot obtains stable posture estimation when and after startup.

[0005] In order to achieve the above technical objectives, the technical solution adopted by the present invention is:

[0006] The present invention provides a repositioning method based on 3D laser radar and a priori map, comprising the following steps:

[0007] Step 1: perform point cloud preprocessing and point cloud segmentation on the lidar data and the inertial measurement unit data to obtain the current point cloud to be registered;

[0008] Step 2: Create a multi-resolution pyramid grid map based on the 2.5D grid map;

[0009] Step 3: Determine whether it is necessary to perform initial registration on the current point cloud to be registered. If yes, proceed to step 4; if not, it means that the initial repositioning has been completed, obtain the initial repositioning result, and proceed to step 5;

[0010] Step 4: Perform 2D coarse registration based on branch and bound search according to the pyramid grid map and the current point cloud to be registered, and then perform 3D fine registration based on ICP registration according to the result of 2D coarse registration, the current point cloud to be registered and the 3D point cloud map. After the iterative optimization converges, complete the initial repositioning and obtain the initial repositioning result when the robot is turned on;

[0011] Step 5: Perform incremental frame-image registration based on the initial relocalization result and the 3D point cloud map to obtain the continuous relocalization result after the robot is turned on.

[0012] Furthermore, the step 1 specifically includes:

[0013] Step 11: aligning and downsampling the laser radar data and the inertial measurement unit data;

[0014] Step 12: perform pose prediction based on the inertial measurement unit data, and estimate the lidar pose of each point in a frame of point cloud in the lidar data relative to the pose at the end of the frame; determine the motion of the point cloud based on the calculated lidar pose, and remove the point cloud with motion distortion.

[0015] Step 13: Divide the preprocessed point cloud by a point cloud segmentation algorithm to obtain a ground point cloud and a wall-like point cloud, and use the ground point cloud and the wall-like point cloud as the current point cloud to be registered.

[0016] Furthermore, the step 2 specifically includes:

[0017] Step 21: Create a 2.5D grid map Based on this, a multi-resolution pyramid raster map is established ;in, Represents the level of the pyramid grid map. is the position of each grid center relative to the origin, in meters;

[0018] Step 22: For layer 0 , by 2.5D grid map Get, the calculation formula is as follows:

[0019] (1)

[0020] in, express Corresponding The ground segmentation information at When it is less than or equal to the threshold 0, it is judged as a ground point cloud. Set to -1; when When it is greater than the threshold 0, it is judged as a wall-like point cloud. Set to 1; if no value is set, Set to 0;

[0021] Step 23: The formula for level calculation is as follows:

[0022] (2)

[0023] Step 24: Perform the following calculations on the remaining layers, distinguishing between walls and floors:

[0024] (3)

[0025] Step 25: Use 2.5D grid map The original ground segmentation information judge Layer, reuse Iterate the subsequent layers to generate a multi-resolution pyramid raster map with ground segmentation information .

[0026] Furthermore, the pyramid grid map has four levels, namely .

[0027] Furthermore, in step 4, a 2D coarse registration based on branch and bound search is performed according to the pyramid grid map and the current point cloud to be registered, specifically including:

[0028] Step 41: After completing the construction of the multi-resolution pyramid grid map, The parameters of each layer are divided into three parts: ground point cloud, wall-like point cloud and unknown point cloud;

[0029] Step 42: Establish an initial possible position set , the initial possible position set Each element in All include scores , Level and posture Information; among which, For elements The corresponding score, For elements The corresponding level, For elements Corresponding 2D pose information; for The translation dimension of the coarsest resolution map Select For the position of -1; The rotation dimension is sampled at equal intervals at a fixed resolution within 360 degrees;

[0030] Step 43: segment the ground point cloud according to the frame point cloud information after point cloud segmentation Point cloud of wall surface Perform multi-resolution construction and use voxel downsampling to generate multi-resolution ground two-dimensional point sets and multi-resolution wall-like 2D point sets ;

[0031] Step 44: Elements , calculate the score corresponding to the posture, and first perform posture transformation according to the following formula:

[0032] (4)

[0033] Get a set of two-dimensional points after translation and rotation transformation and ,for Existing in Points , assuming that the corresponding The number of ground point clouds is Similarly, obtain Corresponding parameters and ,but The calculation formula is as follows:

[0034] (5)

[0035] In the formula, is the positive weight of the ground point cloud, is the positive weight of the wall-like point cloud, is a negative weight,

[0036] Indicates the corresponding level score gain;

[0037] Step 45: After completing the initialization phase, enter the iteration phase, establish a branch-and-bound search algorithm based on depth-first, and obtain the initial possible position set according to the branch-and-bound search algorithm. The element with the highest score .

[0038] Furthermore, in step 45, the initial possible position set is obtained according to the branch and bound search algorithm. The element with the highest score , specifically including:

[0039] Step 451: Set the optimal score , the optimal element ;

[0040] Step 452: Based on the initial possible position set Arrange the scores of the elements in and build a stack , the top of the stack is the element with the highest score;

[0041] Step 453: When the stack When not empty, on the stack Push the element from the top of the stack ;

[0042] Step 454: Determination Is it greater than ,in, Representation elements The corresponding score, Represents the set of possible initial positions If yes, go to step 456; if no, go to step 457;

[0043] Step 456: Determine the element Is it a branch and bound leaf node? If so, update the optimal score. and the optimal element ; If not, proceed to step 457;

[0044] Step 457: Elements Branch and get the collection ; for the set The elements in are fractionally calculated using formula (5); Push onto stack , and sort them according to the score calculation results;

[0045] Step 458: Obtain the optimal position of the leaf node according to the sorting result, which is the initial possible position set. The element with the highest score .

[0046] Furthermore, in step 4, 3D fine registration based on ICP registration is performed according to the result of 2D coarse registration, the current point cloud to be registered and the 3D point cloud map, specifically including:

[0047] Step 46: Get the element with the highest score After that, combined with the 2.5D grid map The corresponding value, Represents a 2.5D grid map The height information in the 3D pose is obtained , in 3D pose As the initial pose for ICP registration, the ground point cloud Point cloud of wall surface Constructing the registration frame point cloud , as shown below:

[0048] (6)

[0049] Step 47: 3D point cloud map after filtering by map filter is the map point cloud, Perform ICP registration between frames and images to obtain the registered pose , and calculate the corresponding 3D registration score , and at the same time and 3D registration score The result of composition Save to collection ; Among them, the 3D registration score The specific calculation method is:

[0050] Through posture Get the global coordinate system Point cloud below , the point cloud Included points, in the 3D point cloud map Medium Calculation The nearest neighbor distance of each point in is , Represents the index of the point, when When it is smaller than the map resolution, it is considered an interior point and is assumed to exist. The final registration score is It is expressed as:

[0051] (7)

[0052] Step 48: Pass the element with the highest score Restricted to the initial set of possible positions The search area in the loop is iterated until the convergence condition is reached. or the maximum number of iterations is reached, Represents the registration score threshold, and finally selects The pose corresponding to the element with the highest score is used as the initial alignment pose , which is the initial relocation result.

[0053] Furthermore, the step 5 specifically includes:

[0054] Step 51: Initial alignment pose Convert to IMU coordinate system In the following, IMU stands for inertial measurement unit, which is used to obtain the initial relocation posture of the current robot. , this posture That is, as the starting pose for continuous relocalization ;

[0055] Step 52: For the current Frame point cloud, after the inertial measurement unit prediction process and point cloud dedistortion processing, the inertial measurement unit predicted pose is obtained And dedistorted point cloud , dedistorted point cloud Obtain ground point cloud through point cloud segmentation algorithm Point cloud of wall surface The combination of the two constitutes the registration frame point cloud of the relocalization link , as shown below:

[0056] (8)

[0057] Step 53: In 3D point cloud map Perform frame-image registration in the process, and then perform iterative error Kalman filtering to obtain the updated pose , which is the continuous relocation result.

[0058] The present invention also provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the repositioning method based on 3D laser radar and a priori map as described above is implemented.

[0059] The present invention also provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the above-mentioned repositioning method based on 3D laser radar and a priori map.

[0060] By adopting the above technical solution, the present invention has the following beneficial effects compared with the prior art:

[0061] The present invention uses multi-resolution rasterization to avoid using multiple layers. The problem of low-resolution maps losing features due to information; through branch-and-bound-based optimization registration, the spatial information of the ground and wall-like surfaces is added, greatly improving the accuracy and convergence speed of registration; through ICP-based optimization registration, the registration accuracy is improved; through the filtering-based continuous relocation method, the influence of dynamic obstacles in the relocation process is eliminated, ensuring the stability of the relocation system. The present invention is a 2D+3D combined, coarse-to-fine global initial alignment and continuous relocation method, which improves the efficiency of calculation while ensuring robustness, and effectively ensures that the robot obtains stable posture estimation when and after startup. BRIEF DESCRIPTION OF THE DRAWINGS

[0062] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0063] Figure 1 It is an execution flow chart of a repositioning method based on 3D laser radar and a priori map provided in an embodiment of the present invention.

[0064] Figure 2 It is a schematic diagram of multi-layer calculation provided by an embodiment of the present invention.

[0065] Figure 3 It is a schematic diagram of a multi-resolution pyramid grid map provided by an embodiment of the present invention.

[0066] Figure 4 It is a schematic diagram of an experimental verification platform provided by an embodiment of the present invention.

[0067] Figure 5 It is a schematic diagram of the initial relocation experiment point selection provided by an embodiment of the present invention.

[0068] Figure 6 This is the first initial relocation experiment result provided by the embodiment of the present invention.

[0069] Figure 7 This is the second initial relocation experiment result provided by the embodiment of the present invention.

[0070] Figure 8 It is a trajectory diagram of a continuous repeated positioning accuracy experiment provided by an embodiment of the present invention.

[0071] Fig. 9 It is a schematic diagram of an electronic device provided by an embodiment of the present invention.

[0072] Fig.10 It is a schematic diagram of a computer-readable storage medium provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0073] The present invention will be further described in detail below in conjunction with the accompanying drawings and examples. It is particularly noted that the following examples are only used to illustrate the present invention, but are not intended to limit the scope of the present invention. Similarly, the following examples are only partial embodiments of the present invention rather than all embodiments, and all other embodiments obtained by those of ordinary skill in the art without creative work are within the scope of protection of the present invention.

[0074] See also Figure 1 The present invention provides a relocation method based on 3D laser radar and a priori map, wherein the priori map includes a 2.5D grid map and a 3D point cloud map, and specifically includes the following steps:

[0075] Step 1: perform point cloud preprocessing and point cloud segmentation on the lidar data and the inertial measurement unit data to obtain the current point cloud to be registered;

[0076] In this embodiment, the step 1 specifically includes:

[0077] Step 11: aligning and downsampling the laser radar data with the inertial measurement unit (IMU) data;

[0078] Step 12: perform pose prediction based on the inertial measurement unit data, and estimate the lidar pose of each point in a frame of point cloud in the lidar data relative to the pose at the end of the frame; determine the motion of the point cloud based on the calculated lidar pose, and remove the point cloud with motion distortion.

[0079] Step 13: Divide the preprocessed point cloud by a point cloud segmentation algorithm to obtain a ground point cloud and a wall-like point cloud, and use the ground point cloud and the wall-like point cloud as the current point cloud to be registered.

[0080] Step 2: Create a multi-resolution pyramid grid map based on the 2.5D grid map;

[0081] In this embodiment, step 2 specifically includes:

[0082] Step 21: Considering the real-time nature of the global alignment algorithm, a 2.5D grid map is created. Based on this, a multi-resolution pyramid raster map is established ;in, Indicates the level of the pyramid grid map. The more levels there are, the faster the convergence speed is. At the same time, the lower the level is, the higher the convergence accuracy is. In practical applications, the level of the pyramid grid map is selected to be four levels, namely , a balance can be achieved between efficiency and accuracy; is the position of each grid center relative to the origin, in meters;

[0083] Step 22: For layer 0 , by 2.5D grid map Get, the calculation formula is as follows:

[0084] (1)

[0085] in, express Corresponding The ground segmentation information at the location is used to determine the map point cloud and the wall-like point cloud; When it is less than or equal to the threshold 0, it is judged as a ground point cloud. Set to -1; when When it is greater than the threshold 0, it is judged as a wall-like point cloud. Set to 1; if no value is set, Set to 0;

[0086] Step 23, the designed map layer is as follows Figure 2 As shown, The formula for level calculation is as follows:

[0087] (2)

[0088] Step 24: Perform the following calculations on the remaining layers, distinguishing between walls and floors:

[0089] (3)

[0090] Step 25: Use 2.5D grid map The original ground segmentation information judge Layer, reuse Iterate the subsequent layers to generate a multi-resolution pyramid raster map with ground segmentation information (like Figure 3 As shown), you can avoid using This information causes the problem of losing features in low-resolution maps.

[0091] Step 3: Determine whether it is necessary to perform initial registration on the current point cloud to be registered. If yes, proceed to step 4; if not, it means that the initial repositioning has been completed, obtain the initial repositioning result, and proceed to step 5;

[0092] Step 4: Perform 2D coarse registration based on branch and bound search according to the pyramid grid map and the current point cloud to be registered, and then perform 3D fine registration based on ICP registration according to the result of 2D coarse registration, the current point cloud to be registered and the 3D point cloud map. After the iterative optimization converges, complete the initial repositioning and obtain the initial repositioning result when the robot is turned on;

[0093] In this embodiment, the step 4 performs a 2D coarse registration based on a branch and bound search according to the pyramid grid map and the current point cloud to be registered, specifically including:

[0094] Step 41: After completing the construction of the multi-resolution pyramid grid map, The parameters of each layer are divided into three parts: ground point cloud (1), wall-like point cloud (-1) and unknown point cloud (0);

[0095] Step 42: Establish an initial possible position set , the initial possible position set Each element in All include scores , Level and posture Information; among which, For elements The corresponding score, For elements The corresponding level, For elements Corresponding 2D pose information; for The translation dimension of the coarsest resolution map Select For the position of -1; The rotation dimension is sampled at equal intervals at a fixed resolution within 360 degrees; in the coarsest resolution map Select Considering the actual application situation, the robot will only exist on the ground by default, so only select The position of -1 reduces the number of sets and improves efficiency; For the rotational dimension, sampling is performed at equal intervals within 360 degrees at a fixed resolution, which can better observe the changing trend of the data and ensure the consistency and accuracy of the data.

[0096] Step 43: segment the ground point cloud according to the frame point cloud information after point cloud segmentation Point cloud of wall surface Perform multi-resolution construction, where F Indicates the ground, W Indicates the wall, L represents laser radar, k Indicatesk Frame laser point cloud; Generate multi-resolution ground 2D point sets using voxel downsampling and multi-resolution wall-like 2D point sets ; The point cloud is downsampled from fine to coarse voxels of different resolutions, and divided into two independent 2D point sets according to the type, which are calculated independently in the subsequent registration score calculation. According to this method, it is equivalent to adding the spatial information of the ground and wall-like surfaces in the projection process from 3D to 2D. Compared with simply selecting points within a range of the z-axis, the accuracy and convergence speed of the registration can be greatly improved.

[0097] Step 44: Elements , calculate the score corresponding to the posture, and first perform posture transformation according to the following formula:

[0098] (4)

[0099] Get a set of two-dimensional points after translation and rotation transformation and ,for Existing in Points , assuming that the corresponding The number of ground point clouds is Similarly, obtain Corresponding parameters and ,but The calculation formula is as follows:

[0100] (5)

[0101] In the formula, is the positive weight of the ground point cloud, is the positive weight of the wall-like point cloud, is a negative weight,

[0102] Indicates the corresponding level score gain; considering that the density of two-dimensional points at different levels is inconsistent, in order to ensure The consistency in different dimensions is still comparable. Added corresponding level score gain .

[0103] Step 45: After completing the initialization phase, enter the iteration phase, establish a branch-and-bound search algorithm based on depth-first, and obtain the initial possible position set according to the branch-and-bound search algorithm. The element with the highest score .

[0104] In this embodiment, the initial possible position set is obtained according to the branch and bound search algorithm in step 45. The element with the highest score , specifically including:

[0105] Step 451: Set the optimal score , the optimal element ;

[0106] Step 452: Based on the initial possible position set Arrange the scores of the elements in and build a stack , the top of the stack is the element with the highest score;

[0107] Step 453: When the stack When not empty, on the stack Push the element from the top of the stack ;

[0108] Step 454: Determination Is it greater than ,in, Representation elements The corresponding score, Represents the set of possible initial positions If yes, go to step 456; if no, go to step 457;

[0109] Step 456: Determine the element Is it a branch and bound leaf node? If so, update the optimal score. and the optimal element ; If not, proceed to step 457;

[0110] Step 457: Elements Branch and get the collection ; for the set The elements in are fractionally calculated using formula (5); Push onto stack , and sort them according to the score calculation results;

[0111] Step 458: Obtain the optimal position of the leaf node according to the sorting result, which is the initial possible position set. The element with the highest score .

[0112] As shown in Algorithm 1:

[0113]

[0114] In this embodiment, in step 4, 3D fine registration based on ICP registration is performed according to the result of 2D coarse registration, the current point cloud to be registered and the 3D point cloud map. This is in consideration of the low robustness of 2D registration, which needs to be improved; specifically, the following steps are included:

[0115] Step 46: Get the element with the highest score After that, combined with the 2.5D grid map The corresponding value, Represents a 2.5D grid map The height information in the 3D pose is obtained , in 3D pose As the initial pose for ICP registration, the ground point cloud Point cloud of wall surface Constructing the registration frame point cloud , as shown below:

[0116] (6)

[0117] Step 47: 3D point cloud map after filtering by map filter is the map point cloud, Perform ICP registration between frames and images to obtain the registered pose , and calculate the corresponding 3D registration score , and at the same time and 3D registration score The result of composition Save to collection ; Among them, the 3D registration score The specific calculation method is:

[0118] Through posture Get the global coordinate system Point cloud below , the point cloud Included points, in the 3D point cloud map Medium Calculation The nearest neighbor distance of each point in is , Represents the index of the point, when When it is smaller than the map resolution, it is considered an interior point and is assumed to exist. The final registration score is It is expressed as:

[0119] (7)

[0120] In the formula, It is expressed as the inlier ratio parameter, which is the ratio of the number of inliers to the total input point cloud; It is expressed as the registration error parameter, which is the mean of the distances between all inliers and their nearest neighbors. A higher score indicates a more accurate registration.

[0121] Step 48: Pass the element with the highest score Restricted to the initial set of possible positions The search area in the loop is iterated until the convergence condition is reached. or the maximum number of iterations is reached, Represents the registration score threshold, and finally selects The pose corresponding to the element with the highest score is used as the initial alignment pose , which is the initial relocation result.

[0122] In summary, the overall framework of the optimized registration algorithm based on branch and bound and ICP is established as shown in Algorithm 2:

[0123]

[0124] Step 5: Perform incremental frame-image registration based on the initial relocalization result and the 3D point cloud map to obtain the continuous relocalization result after the robot is turned on.

[0125] In this embodiment, step 5 specifically includes:

[0126] Step 51: Initial alignment pose Convert to IMU coordinate system In the following, IMU stands for inertial measurement unit, which is used to obtain the initial relocation posture of the current robot. , this posture That is, as the starting pose for continuous relocalization ;

[0127] Step 52: For the current Frame point cloud, after the inertial measurement unit prediction process and point cloud dedistortion processing, the inertial measurement unit predicted pose is obtained And dedistorted point cloud , dedistorted point cloud Obtain ground point cloud through point cloud segmentation algorithm Point cloud of wall surface The combination of the two constitutes the registration frame point cloud of the relocalization link , as shown below:

[0128] (8)

[0129] Step 53: In 3D point cloud map Frame-image registration is performed in Relative to In terms of the number of point clouds, the number of irregular point clouds is reduced, most of the irregular point clouds are eliminated, the success rate of the nearest neighbor selection can be improved, and the efficiency is improved. Then the iterative error Kalman filter is performed to obtain the updated pose , which is the continuous relocation result.

[0130] Although the irregular point clouds are eliminated to the maximum extent after segmentation, dynamic obstacles with regular plane surfaces cannot be eliminated by relying on single frame information. When performing point-to-surface registration in frame-to-image matching, due to Only static wall-like point clouds and ground point clouds are left. Dynamic obstacles in The corresponding plane is found, so it will not affect the pose calculation. This eliminates the influence of dynamic obstacles during the relocalization process and ensures the stability of the relocalization system.

[0131] The present invention provides a relocation method based on 3D laser radar and a priori map, which is used for efficient and robust relocation of mobile robots in indoor environments. The specific implementation process is as follows.

[0132] Step 1: Build the experimental platform and environment.

[0133] like Figure 4 As shown, based on the Scout-mini mobile robot platform, it is equipped with a multi-line laser radar 1 (Robosense RS-LiDAR-16 (20Hz)), an IMU inertial measurement unit 2 (Xsens MTi 30 (400Hz)), an edge computing box 3 (Nvidia Jetson AGX Xavier, ROS Melodic) as the computing platform, and a personal computer (AMD R7-4800H, ROS Noetic).

[0134] Step 2: Implement initial relocation. The initial relocation process is programmed and tested, and the results are as follows.

[0135] To demonstrate the performance of the proposed initial relocalization method, six pose points in the 3D map created in the mapping phase are selected as test benchmarks, such as Figure 5 shown. Figure 5 In the figure, the starting point of the arrow indicates the position, and the direction indicates the posture. Algorithms 1 and 2 proposed in this invention are tested at the above six positions and postures. The experimental results are shown in Figure 2. Figure 6 As shown in Figure 7, the initial registration is completed correctly in all six poses.

[0136] At the same time, the initial alignment method proposed in the present invention is compared with two other methods applicable to 3D point clouds: one is the BBS method modified for three-dimensional point clouds, which selects point clouds within a certain height and compresses them to a plane; the other is the FPFH method based on feature descriptors. The experimental results are shown in Table 1. It can be seen from the table that the BBS method did not complete the registration in poses C and D. This is because there are a large number of long corridors or stairwells similar to the pose in the experimental environment. It is easy to cause registration errors by relying solely on two-dimensional registration; the FPFH method makes incorrect judgments in all poses due to insufficient environmental texture and high repeatability.

[0137] Table 1: Comparison of initial alignment experiments (ms)

[0138]

[0139] In contrast, the proposed method combines two-dimensional and three-dimensional information to enhance robustness. In terms of computational time, thanks to the proposed highly robust ground segmentation method, the initial BBS priority queue is limited, which greatly improves the convergence speed. Compared with the original BBS method, the proposed method takes only 10% of the time.

[0140] Step 3: Implement continuous relocation. The continuous relocation process is programmed and tested, and the results are as follows.

[0141] A ground marking frame was set up on the ground of the experimental environment. The robot was controlled to start from the ground marking frame, pass through different trajectories, and reach the same marking frame four times. The trajectory of each operation process was recorded, such as Figure 8 As shown in the figure, the black dots represent the ground marking frame, and different lines represent different motion trajectories. In the experiment, the positioning position of the robot is obtained every time it reaches the ground marking frame, the standard deviation and total deviation of the four position information are calculated, and the repeated positioning accuracy is experimentally tested.

[0142] As shown in Table 2, the continuous relocation scheme designed by the present invention has a repeated positioning accuracy of 0.5 cm compared with the two comparative relocation schemes, which is significantly better than the existing FAST-LIO2 (M) and HDL methods.

[0143] Table 2: Repeatability test (m)

[0144]

[0145] Real-time performance is the basis for robots to ensure stable operation, and the relocation method of the present invention also has advantages in real-time performance. Time consumption tests were conducted on two computing platforms (personal computer AMD R7-4800H and edge computing box Nvidia Jetson AGXXavier) to evaluate the relocation process of the present invention and the HDL method in the actual environment. For each frame of laser radar (20Hz), the time consumption results of each module are shown in Table 3. Compared with the HDL method, the scheme proposed by the present invention reduces the time consumption by about 50%, and both personal computers and vehicle-mounted computing boxes can meet the real-time requirements.

[0146] Table 3: Average time consumption (ms)

[0147]

[0148] like Fig. 9 As shown, an embodiment of the present invention also provides an electronic device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the above-mentioned repositioning method based on 3D lidar and a priori map when executing the program.

[0149] like Fig.10 As shown, an embodiment of the present invention further provides a computer-readable storage medium on which a computer program is stored. When the program is executed by a processor, the above-mentioned repositioning method based on 3D laser radar and prior map is implemented.

[0150] In addition, each functional unit in each embodiment of the present invention may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit. The above-mentioned integrated unit may be implemented in the form of hardware or in the form of software functional units.

[0151] If the integrated unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or all or part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including a number of instructions for a computer device (which can be a personal computer, server, or network device, etc.) or a processor (processor) to perform all or part of the steps of each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk and other media that can store program codes.

[0152] The above descriptions are only some embodiments of the present invention, and are not intended to limit the protection scope of the present invention. Any equivalent device or equivalent process transformation made using the contents of the present invention specification and drawings, or directly or indirectly applied in other related technical fields, are also included in the patent protection scope of the present invention.

Claims

1. A relocation method based on 3D laser radar and prior map, characterized in that: The steps include: Step 1: perform point cloud preprocessing and point cloud segmentation on the lidar data and the inertial measurement unit data to obtain the current point cloud to be registered; Step 2: Establish a multi-resolution pyramid grid map based on the 2.5D grid map; specifically including: Step 21: Create a 2.5D grid map Based on this, a multi-resolution pyramid raster map is established ;in, Represents the level of the pyramid grid map. is the position of each grid center relative to the origin, in meters; Step 22: For layer 0 , by 2.5D grid map Get, the calculation formula is as follows: （1） in, express Corresponding The ground segmentation information at When it is less than or equal to the threshold 0, it is judged as a ground point cloud. Set to -1; when When it is greater than the threshold 0, it is judged as a wall-like point cloud. Set to 1; if no value is set, Set to 0; Step 23: The formula for level calculation is as follows: （2） Step 24: Perform the following calculations on the remaining layers, distinguishing between walls and floors: （3） Step 25: Use 2.5D grid map The original ground segmentation information judge Layer, reuse Iterate the subsequent layers to generate a multi-resolution pyramid raster map with ground segmentation information ; Step 3: Determine whether it is necessary to perform initial registration on the current point cloud to be registered. If yes, proceed to step 4; if not, it means that the initial repositioning has been completed, obtain the initial repositioning result, and proceed to step 5; Step 4: Perform 2D coarse registration based on branch and bound search according to the pyramid grid map and the current point cloud to be registered, and then perform 3D fine registration based on ICP registration according to the result of 2D coarse registration, the current point cloud to be registered and the 3D point cloud map. After iterative optimization converges, complete the initial repositioning and obtain the initial repositioning result when the robot is turned on; the 2D coarse registration based on branch and bound search according to the pyramid grid map and the current point cloud to be registered specifically includes: Step 41: After completing the construction of the multi-resolution pyramid grid map, The parameters of each layer are divided into three parts: ground point cloud, wall-like point cloud and unknown point cloud; Step 42: Establish an initial position set , the initial position set Each element in All include scores , Level and posture Information; among which, For elements The corresponding score, For elements The corresponding level, For elements Corresponding 2D pose information; for The translation dimension of the coarsest resolution map Select For the position of -1; The rotation dimension is sampled at equal intervals at a fixed resolution within 360 degrees; Step 43: segment the ground point cloud according to the frame point cloud information after point cloud segmentation Wall-like point cloud Perform multi-resolution construction and use voxel downsampling to generate multi-resolution ground two-dimensional point sets and multi-resolution wall-like 2D point sets ; Step 44: Elements , calculate the score corresponding to the posture, and first perform posture transformation according to the following formula: （4） Get a set of two-dimensional points after translation and rotation transformation and ,for Existing in Points , assuming that the corresponding The number of ground point clouds is Similarly, obtain Corresponding parameters and ,but The calculation formula is as follows: （5） In the formula, is the positive weight of the ground point cloud, is the positive weight of the wall-like point cloud, is a negative weight, Indicates the corresponding level score gain; Step 45: After completing the initialization phase, enter the iteration phase, establish a branch-and-bound search algorithm based on depth-first, and obtain the initial position set according to the branch-and-bound search algorithm. The element with the highest score ; Step 5: Perform incremental frame-image registration based on the initial relocalization result and the 3D point cloud map to obtain the continuous relocalization result after the robot is turned on.

2. The relocation method based on 3D laser radar and prior map according to claim 1, characterized in that: The step 1 specifically includes: Step 11: aligning and downsampling the laser radar data and the inertial measurement unit data; Step 12: perform pose prediction based on the inertial measurement unit data, and estimate the lidar pose of each point in a frame of point cloud in the lidar data relative to the pose at the end of the frame; determine the motion of the point cloud based on the calculated lidar pose, and remove the point cloud with motion distortion; Step 13: Divide the preprocessed point cloud by a point cloud segmentation algorithm to obtain a ground point cloud and a wall-like point cloud, and use the ground point cloud and the wall-like point cloud as the current point cloud to be registered.

3. The relocation method based on 3D laser radar and prior map according to claim 1, characterized in that: The pyramid grid map has four levels, namely .

4. The relocation method based on 3D laser radar and prior map according to claim 1, characterized in that: In step 45, the initial position set is obtained according to the branch and bound search algorithm. The element with the highest score , specifically including: Step 451: Set the optimal score , the optimal element ; Step 452: According to the initial position set Arrange the scores of the elements in and build a stack , the top of the stack is the element with the highest score; Step 453: When the stack When not empty, on the stack Push the element from the top of the stack ; Step 454: Determination Is it greater than ,in, Representation elements The corresponding score, Represents the initial position set If yes, go to step 456; if no, go to step 457; Step 456: Determine the element Is it a branch and bound leaf node? If so, update the optimal score. and the optimal element ; If not, proceed to step 457; Step 457: Elements Branch and get the collection ; for the set The elements in are fractionally calculated using formula (5); Push onto stack , and sort them according to the score calculation results; Step 458: Obtain the optimal position of the leaf node according to the sorting result, which is the initial position set. The element with the highest score .

5. The relocation method based on 3D laser radar and prior map as claimed in claim 4, characterized in that: In step 4, 3D fine registration based on ICP registration is performed according to the result of 2D coarse registration, the current point cloud to be registered and the 3D point cloud map, specifically including: Step 46: Get the element with the highest score After that, combined with the 2.5D grid map The corresponding value, Represents a 2.5D grid map The height information in the 3D pose is obtained , in 3D pose As the initial pose for ICP registration, the ground point cloud Wall-like point cloud Constructing the registration frame point cloud , as shown below: （6） Step 47: 3D point cloud map after filtering by map filter is the map point cloud, Perform ICP registration between frames and images to obtain the registered pose , and calculate the corresponding 3D registration score , and at the same time and 3D registration score The result of composition Save to collection ; Among them, the 3D registration score The specific calculation method is: Through posture Get the global coordinate system Point cloud below , the point cloud Included points, in the 3D point cloud map Medium Calculation The nearest neighbor distance of each point in is , Represents the index of the point, when When it is smaller than the map resolution, it is considered an interior point and is assumed to exist. The final registration score is It is expressed as: （7） Step 48: Pass the element with the highest score Restricted to the initial position set The search area in the loop is iterated until the convergence condition is reached. or the maximum number of iterations is reached, Represents the registration score threshold, and finally selects The pose corresponding to the element with the highest score is used as the initial alignment pose , which is the initial relocation result.

6. The relocation method based on 3D laser radar and prior map according to claim 5, characterized in that: The step 5 specifically includes: Step 51: Initial alignment pose Convert to IMU coordinate system In the following, IMU stands for inertial measurement unit, which is used to obtain the initial relocation posture of the current robot. , this posture That is, as the starting pose for continuous relocalization ; Step 52: For the current Frame point cloud, after the inertial measurement unit prediction process and point cloud dedistortion processing, the inertial measurement unit predicted pose is obtained And dedistorted point cloud , dedistorted point cloud Obtain ground point cloud through point cloud segmentation algorithm Wall-like point cloud The combination of the two constitutes the registration frame point cloud of the relocalization link , as shown below: （8） Step 53: In 3D point cloud map Perform frame-image registration in the process, and then perform iterative error Kalman filtering to obtain the updated pose , which is the continuous relocation result.

7. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the program, the repositioning method based on 3D lidar and a priori map is implemented as described in any one of claims 1 to 6.

8. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the repositioning method based on 3D laser radar and a priori map as described in any one of claims 1 to 6 is implemented.

Images