A grid map-based brute-force matching loop closure detection method

Abstract

A kind of grid map-based brute force matching loop detection method, comprising: S1: obtaining laser sensor data, due to the distortion of point cloud data caused by object motion, correct laser point cloud data;S2: the initial data obtained is projected in two dimensions, and the two-dimensional point cloud after projection is pixelated;S3: ensure the accuracy of initial map, whether the initial map is closed is judged using breadth-first search detection;S4: update the maximum likelihood estimation of each grid in the grid map, and construct the maximum grid probability map, while calculating all grid coordinates between hit point and original point;S5: start front-end adjacent matching optimization calculation pose, start brute force matching, extract the rotation amount and displacement amount based on two-dimensional plane grid map coordinate system based on motion estimation under absolute coordinate system.The present application can overcome the error key frame stacking problem caused by cumulative error in existing loop detection in closed environment, ensure the accuracy of positioning system.

Description

TECHNICAL FIELD

[0001] The present application relates to a loop detection algorithm inside a closed environment, which uses optimized pose and laser information to perform a brute force matching based on a grid map, and obtains loop detection optimization of accumulated errors. BACKGROUND

[0002] Simultaneous Localization And Mapping (SLAM) perceives the surrounding environment through sensors, and performs self-positioning according to the calculated position and map, and simultaneously constructs an incremental map to realize autonomous positioning and navigation. Through the joint action of front-end odometry and back-end optimization, positioning in a position environment can be realized, but since the pose transformation is calculated through least square optimization, and the positioning problem to be solved is a complex nonlinear system, the final optimization result may fall into a local minimum value, i.e. positioning error, for example, a sudden movement at a certain moment will cause positioning drift; and if the insertion of a wrong key frame pose into the point cloud map is selected, this will cause a large error in the matching between the frame and the map during the subsequent back-end optimization, and even cause complete failure of positioning. Loop detection can improve the positioning accuracy of the system, and timely correct the pose when a repeated scene appears, and optimize the simultaneous localization and mapping. Most of the current loop detection methods are bag-of-words models, which are a simple document representation method through natural language processing and information retrieval, and have a large amount of calculation in a large scene, are prone to cause mismatching, and have low efficiency and poor real-time performance. SUMMARY

[0003] The present application aims to overcome the problem of incorrect key frame stacking caused by accumulated errors in the existing technology for loop detection in a closed environment, and proposes a brute force matching loop detection method based on a grid map.

[0004] The technical solution adopted by the present application to solve the problems of the prior art is:

[0005] A brute force matching loop detection method based on a grid map, comprising the following steps:

[0006] S1: Obtain laser sensor data, and correct the laser point cloud data due to distortion of the point cloud data caused by object movement;

[0007] S2: Perform two-dimensional projection on the obtained initial data, and perform pixelization processing on the projected two-dimensional point cloud;

[0008] S3: Ensure the accuracy of the initial map, and use breadth-first search to detect whether the initial map is closed for judgment;

[0009] S4: update the maximum likelihood estimation of each grid in the grid map, and construct the maximum grid probability map, while calculating all grid coordinates between the hit point and the origin;

[0010] S5: start the front-end adjacent matching optimization calculation pose, start the brute force matching, and extract the rotation and displacement based on the two-dimensional plane grid map coordinate system based on the motion estimation in the absolute coordinate system.

[0011] Preferably, the step S1 specifically comprises:

[0012] P k+i ,P k+i ' represents the i-th distortion point and the corrected point in the k-th frame; since the IMU unit measurement frequency is 10 times that of the laser radar, each laser point can be calculated by the IMU information interpolation to obtain the current pose transformation matrix of the corresponding timestamp, and the radar point cloud is processed by interpolation according to the uniform speed model, and the pose transformation matrix is:

[0013]

[0014] w, j is the motion estimation frame number of all IMUs in the k-th frame and the j-th frame in which the current point is located, T k+j ,T k+j+1 is the change matrix of the current k-th frame superimposed with the change matrix of the current IMU motion estimation and the change matrix of the next frame.

[0015] Preferably, the step S2 specifically comprises:

[0016] The initial number of frames of corrected point cloud of the radar starting static scanning is constructed into a point cloud map, and then projected onto a two-dimensional plane. The projected two-dimensional point cloud is pixelated, and then the point parameter value falling into the pixel is subjected to the weighted operation of formula (2), and the value is taken as the corresponding pixel estimation and marked as an occupied pixel to avoid the phenomenon of repeated hit score of the same pixel in the subsequent process;

[0017]

[0018] Where S i represents the weight of the current point, w i represents the parameter value of the current point, and k represents the weighting factor. The larger the setting is, the smaller the point is, and the larger the weight is.

[0019] Preferably, the step S3 specifically comprises:

[0020] S3.1: obtaining the two-dimensional plane in S2, taking the initial state as the starting point, and storing in the queue;

[0021] S3.2: Take the first pixel coordinate from the queue and set it as the current pixel, and mark this pixel as a searched state. Then search the unsearched pixels in the four neighborhoods of the current pixel.

[0022] S3.3: If the searched pixel is in a black occupied state, skip this step; if the searched pixel is in an unoccupied state, push the searched pixel coordinates to the end of the queue; if the map boundary is encountered during the search, it means that the map is not completely closed, otherwise repeat step S3.2.

[0023] S3.4: If the final queue is empty, but no map boundary is encountered during the pixel search, it means that the occupied pixels form a closed space, indicating that the map is closed at this time. Stop updating the pixel map and save the map.

[0024] Preferably, step S4 specifically includes:

[0025] After detecting closures in the initial 2D planar pixel map constructed in S2 using S3, the initial point cloud map constructed in S2 is projected onto the raster map to construct an initial matching raster map. The overlay raster map updates the maximum likelihood estimate by checking whether the raster hits or misses, thus achieving a dynamic update of the point cloud map. After obtaining the overlaid and classified point cloud, each frame of the point cloud is used as prior data and input into the raster map.

[0026] data={x1,T1,x2,T2,…,x n ,T n} (3)

[0027] data represents the current point cloud frame information, x n ,T n Represents the coordinates and pose of the nth point;

[0028] To generate a raster map that most likely matches the information data from the current frame and historical frames:

[0029] m * =arg max m P(m|data) (4)

[0030] During the initial point cloud frame overlay process, the maximum likelihood estimate of each grid cell in the grid map is updated synchronously using formula (5), the prior point cloud data in formula (3) is updated at the same time, and the maximum grid probability map in formula (4) is constructed. After the construction is completed, it is used as the initial map for brute-force matching.

[0031]

[0032] Calculate all grid coordinates between the hit point and the origin, as shown in formula (6):

[0033]

[0034] where Δx and Δt represent the distance between the hit point and the origin, x i represents the x coordinate of the hit point, then the sign of Δd can be determined to determine the specific value of the grid coordinate y, as formula (7):

[0035]

[0036] Preferably, the step S5 specifically comprises:

[0037] S5.1: by matching between frames, a nonlinear constraint equation set of point lines and point planes is constructed, and the equation set is solved by the LM method to obtain the solved radar pose; and by matching between frames and maps, a nonlinear constraint equation set of point lines and point planes is constructed, and the LM method is used again to solve to obtain the optimized motion estimation;

[0038] S5.2: first, the rotation and displacement based on the two-dimensional plane grid map coordinate system are extracted from the motion estimation based on the absolute coordinate system, i.e. T = (x, y, θ); then n three-dimensional matching windows are created, i.e. T i = (x i ,y i ,θ i ), by pose superposition, the matched point cloud is obtained, and the point cloud is projected onto the two-dimensional grid to do score calculation with the initial grid constructed in 6), if the point falls into the occupied grid, the score increases, and if the point falls into the idle grid, the score decreases, as formula (8):

[0039]

[0040] where M(T i ) represents the grid score under the window T i matching, and F(T) is the highest score matching result from all window superpositions;

[0041] S5.3: according to formula (8), the translation and rotation of F(T) are selected, and the highest score is evaluated, if it is greater than the set threshold, it is considered that the current window matching result meets the requirements, and the pose calculation of the key frame meets the algorithm requirements, and the window T i = (x i ,y i ,θ i ) at this time is superimposed into the pose calculated by the front-end matching optimization, and the key frame is allowed to enter the historical point cloud information.

[0042] The application provides a grid map-based brute force matching loop detection method, which detects the pose calculation accuracy of a key frame to provide a key frame detection method for a rear-end loop detection.

[0043] At present, many methods utilize the distribution of calculated poses on a global map to determine key frames, or implement loop detection based on the Euclidean distance, so that once a large motion occurs and an error pose calculation result is generated, certain errors exist in the entire loop detection, and cannot be removed in subsequent matching, resulting in the failure of the entire positioning.

[0044] The application uses optimized poses and laser information to perform grid map-based brute force matching, obtains loop detection optimization of cumulative errors, and guarantees the accuracy of a positioning system by screening key frames and superimposed window poses.

[0045] The application determines the stacking basis of the key frames by evaluating whether the pose has errors through the grid score of the point cloud information projected by the point cloud information after the pose matching, guarantees the accuracy of the entire loop detection key frame, avoids the optimization result from falling into a local minimum value, and improves the accuracy of the front-end and rear-end optimization calculation.

[0046] The application has the advantages that the accuracy of the entire loop detection key frame is guaranteed, loop detection optimization of cumulative errors is obtained, and the accuracy of a positioning system is guaranteed by screening key frames and superimposed window poses. DETAILED DESCRIPTION

[0047] Figure 1 is a flow structure schematic diagram of the application.

[0048] Figures 2a to 2d is a breadth-first search step diagram of the application, wherein, Figure 2a is a car initial plane diagram, Figure 2b is a breadth search schematic diagram, Figure 2c is a boundary failure schematic diagram, Figure 2d is a search success schematic diagram.

[0049] Figure 3 is an idle point updating schematic diagram of the application.

[0050] Figures 4a to 4c is a complete mapping step diagram of the application, wherein, Figure 4a is a mapping initial state diagram, Figure 4b is a mapping intermediate state diagram, Figure 4c is a mapping completion state diagram.

[0051] Figure 5 is a final three-dimensional mapping effect diagram of the application. DETAILED DESCRIPTION

[0052] With reference to the drawings and the embodiments of the present application, the technical solutions in the embodiments of the present application will be described clearly and completely. Obviously, the described embodiments are only a part of the embodiments of the present application, rather than all the embodiments of the present application. Based on the embodiments of the present application, all other embodiments obtained by those skilled in the art without creative efforts belong to the scope of the present application.

[0053] The present embodiment relates to a cabin three-dimensional mapping method based on a grid map and a violent matching loop detection method of the present application, which comprises the following steps:

[0054] Step S1: data acquisition and processing, correcting the acquired cabin data:

[0055] The data is acquired by radar scanning of the empty cabin at the bulk cargo terminal, and the IMU data is collected simultaneously by the moving three-dimensional radar. Each acquired data is interpolated and corrected by formula (1):

[0056]

[0057] w, j is the motion estimation frame number of all IMUs in the kth frame and the jth frame where the current point is located, T k+j ,T k+j+1 is the change matrix of the current kth frame, which is superimposed with the change matrix of the motion estimation of the current IMU and the change matrix of the next frame;

[0058] Step S2: projecting the corrected cabin laser point cloud data and performing pixelization processing on the two-dimensional point cloud:

[0059] The initial frames of point cloud corrected after the start of radar static scanning are used to construct a point cloud map, which is then projected onto a two-dimensional plane. The projected two-dimensional point cloud is pixelized, and the parameter value of the point falling into the pixel is subjected to weighted operation of formula (2), which is taken as the estimated value of the corresponding pixel and is marked as an occupied pixel to avoid the phenomenon of repeated hitting score of the same pixel in the subsequent process;

[0060]

[0061] Where S i represents the weight of the current point, w i represents the parameter value of the current point, and k represents the weighting factor. The larger the setting is, the smaller the point is, and the larger the weight is;

[0062] Step S3: detecting the closed state of the cabin point cloud data:

[0063] S3.1: as shown in Fig. 2(a), after obtaining the two-dimensional plane in the capture, the initial state is taken as the starting point, stored in the queue, and the search is initially started from the point;

[0064] S3.2: As shown in Figure 2(b), the first pixel coordinate is taken from the queue and set as the current pixel, and this pixel is marked as a searched state. Then, the unsearched state pixels in the four neighborhoods of the current pixel are searched.

[0065] S3.3: If the searched pixel is in a black occupied state, skip this step; if the searched pixel is in an unoccupied state, push the searched pixel coordinates to the end of the queue. As shown in Figure 2(c), if the map boundary is encountered during the search, it means that the map is not completely closed; otherwise, repeat step 4-2.

[0066] S3.4: As shown in Figure 2(d), if the final queue is empty, but the map boundary is not encountered during the pixel search process, it means that the occupied pixels form a closed space, indicating that the cabin map is closed at this time. Stop updating the pixel map and save the map.

[0067] Step S4: Construct maximum likelihood estimation, raster map, and raster coordinates for the ship's cabin data:

[0068] After detecting closures in the initial 2D planar pixel map constructed in S2 using S3, the initial point cloud map constructed in S2 is projected onto the raster map to construct an initial matching raster map. The overlay raster map updates the maximum likelihood estimate by checking whether the raster hits or misses, thus achieving a dynamic update of the point cloud map. After obtaining the overlaid and classified point cloud, each frame of the point cloud is used as prior data and input into the raster map.

[0069] data={x1,T1,x2,T2,…,x n ,T n} (3)

[0070] data represents the current point cloud frame information, x n ,T n Represents the coordinates and pose of the nth point;

[0071] To generate a raster map that most likely matches the information data from the current frame and historical frames:

[0072] m * =arg max m P(m|data) (4) During the initial point cloud frame overlay process, the maximum likelihood estimate of each grid in the grid map is updated synchronously with formula (5), the prior point cloud data in formula (3) is updated at the same time, and the maximum grid probability map in formula (4) is constructed. After the construction is completed, it is used as the initial map for brute-force matching.

[0073]

[0074] All grid coordinates between the hit point and the origin are calculated, as shown in equation (6):

[0075]

[0076] where Δx and Δt represent the distance between the hit point and the origin, x i represents the x coordinate of the hit point, then the positive and negative of Δd can be determined to determine the specific value of the grid coordinate y, as shown in equation (7):

[0077]

[0078] As Figure 3 shown in the schematic diagram, all free point coordinates between the origin can be obtained through a black hit point coordinate, and the maximum likelihood estimation of this part is updated according to equation (5). Similarly, after obtaining all the free grid coordinates, the maximum likelihood estimation of all free grids in the initial grid map is updated. As shown in FIG. 4(a), a matching grid map constructed from the initial laser information can be obtained, and the internal space is constructed by a series of bright and dark lines, which is the free point coordinate calculated by the Bresenham algorithm. The dark one has no corresponding hit point, but as shown in FIG. 4(b), it can be seen that the inside of the space gradually brightens, because the car moves forward to update many hit points, thereby updating the internal free points, and finally in FIG. 4(c), the entire internal space is a free point.

[0079] Step S4: Obtain the pose of adjacent data and perform brute-force matching loop detection:

[0080] S5.1: Through the matching between frames, the nonlinear constraint equation set of point lines and point surfaces is constructed, and the equation set is solved by the LM method to obtain the solved radar pose; and through the matching between frames and the map, the nonlinear constraint equation set of point lines and point surfaces is constructed, and the LM method is used again to solve to obtain the optimized motion estimation;

[0081] S5.2: First, the rotation and displacement based on the two-dimensional plane grid map coordinate system are extracted from the motion estimation based on the absolute coordinate system, that is, T=(x, y, θ); then n three-dimensional matching windows are created, that is, T i =(x i ,y i ,θ i ), the matched point cloud is obtained through pose superposition, and the point cloud is projected onto the two-dimensional grid to do score calculation with the initial grid constructed in 6), if the point falls into the occupied grid, the score increases, if the point falls into the free grid, the score decreases, as shown in equation (8):

[0082]

[0083] where M(T i ) represents the grid score of the window T i under the matching, and F(T) is the highest score matching result from all window overlays;

[0084] S5.3: According to formula (8), the translation and rotation of F(T) are selected, the highest score is evaluated, if it is greater than the set threshold, it is considered that the current window matching result meets the requirements, and the pose calculation of the key frame meets the algorithm requirements, and the window T i =(x i ,y i ,θ i ) is superimposed into the pose calculated by the front-end matching optimization, and the key frame is allowed to enter the historical point cloud information;

[0085] Step S6: According to the brute force matching loop detection result, a cabin map is constructed:

[0086] S6.1: After the brute force matching, a three-dimensional mapping operation is performed, and the three-dimensional and grid map of the cabin is updated according to the correct key frame point cloud. Through the motion estimation after the superimposed window in formula (9) and the key cabin map information based on the radar coordinate system:

[0087]

[0088] where T(x g ,y g ,θ g ) represents the pose transformation matrix from the world coordinate system to the grid coordinate system, and the spatial coordinates are converted into the grid coordinates in the grid map, as shown in formula (10):

[0089]

[0090] S6.2: After the key frame point cloud is projected into the grid map, the hit point grid probability is updated by using formula (5), the coordinates of all idle points in the space are determined by using formula (7), the maximum probability grid map of the updated (4) is saved, and is used as the basis for the next key frame matching. The three-dimensional mapping effect of the cabin by the brute force matching loop detection method is shown in Figure 5 , the outer red part is formed by the hits in the closed environment, the middle white point is the historical pose of the car, and the upper green part is the constructed cabin environment map. Through the brute force matching of the point cloud, the accuracy of the key frame pose can be obtained, so that the matching and historical information are optimized, and it can be seen that the pose calculation and three-dimensional map by the brute force matching are accurate.

[0091] The cabin mapping part in the closed environment can be applied in the embodiment of the application, and the accuracy of the original algorithm and the further matching accuracy of the loop detection can be improved by the external hanging mode.

Claims

1. A grid map based brute-force matching loop closure detection method, characterized in that: Comprise the following steps: S1: obtain laser sensor data, due to the existence of point cloud data distortion caused by object motion, correct the laser point cloud data; S2: the initial data obtained by two-dimensional projection, the projected two-dimensional point cloud is pixelated; S3: ensure the accuracy of the initial map, using breadth first search to detect whether the initial map is closed and to judge. S4: update the maximum likelihood estimation of each grid in the grid map, and construct the maximum grid probability map as the initial grid map of brute force matching, and calculate all grid coordinates between the hit point and the origin; S5: start the front end adjacent matching optimization calculation pose, start brute force matching, extract the rotation and displacement based on the two-dimensional plane grid map coordinate system based on the motion estimation under the absolute coordinate system; specifically including: S5.1: through the matching between frames, the point line and point surface nonlinear constraint equation set is constructed, and the radar pose is obtained by solving the equation set by LM method; and through the matching between frames and maps, the point line and point surface nonlinear constraint equation set is constructed, and the optimized motion estimation is obtained by solving again by LM method; S5.2: Firstly, the rotation and displacement based on the two-dimensional plane grid map coordinate system are extracted based on the motion estimation in the absolute coordinate system, that is ; then, three-dimensional matching windows are created, that is , the matched point cloud is obtained through pose superposition, and the point cloud is projected onto the two-dimensional grid, and the score is calculated with the initial grid map constructed in step S4. If the point falls into the occupied grid, the score increases, and if the point falls into the idle grid, the score decreases, as formula (8): ​ wherein representing the highest scoring match under the window match, from all window overlays is the highest scoring match result from all window overlays; S5.3: According to formula (8), the translation and rotation amount of the current window is selected The highest score is evaluated. If it is greater than the set threshold value, it is considered that the current window matching result meets the requirements, and the pose calculation of the key frame meets the algorithm requirements. At this time, the window is superimposed into the pose calculated by the front-end matching optimization, and the key frame is allowed to enter the historical point cloud information.

2. The grid map based brute-force matching loop closure detection method of claim 1, wherein: The step S1 specifically comprises: representing the first intra-frame first distortion point and correction point; since the IMU unit measurement frequency is 10 times that of the laser radar, each laser point can be calculated by the IMU information interpolation to obtain the current pose transformation matrix corresponding to the timestamp, and the radar point cloud is processed by interpolation according to the uniform speed model, and the pose transformation matrix is: , is the first frame number of motion estimation of all IMUs in the frame and the first frame number in which the current point is located, is the change matrix of the current first frame superimposed with the change matrix of motion estimation of the current IMU and the change matrix of the next frame.

3. The grid map based brute-force matching loop closing method of claim 1, wherein: The step S2 specifically comprises: The initial number of frames of corrected point cloud of radar starting static scanning is constructed into a point cloud map, then projected onto a two-dimensional plane, and the projected two-dimensional point cloud is pixelated, then the point parameter value falling into the pixel is subjected to the weighted operation of formula (2), and the value is taken as the corresponding pixel estimation and marked as an occupied pixel to avoid the phenomenon of repeated hit score of the same pixel in the subsequent process; wherein a weight size representative of the current point, a parameter value representative of the current point, a weighting factor representative of the point, the greater the setting, the smaller the point representative of, the greater the weight.

4. The grid map based brute-force matching loop closing method of claim 1, wherein: The step S3 specifically comprises: S3.1: obtain the two-dimensional plane in S2, and take the initial state as the starting point and store it in the queue; S3.2: take the first pixel coordinate from the queue as the current pixel, and mark the pixel as a searched state, then search the unsearched state pixels in the four domains of the current pixel; S3.3: if the searched pixel is a black occupied state, skip this step; if the searched pixel is an unoccupied state, push the searched pixel coordinate to the end of the queue; if the map boundary is encountered in the searching process, it indicates that the map is not completely closed, otherwise repeat step S3.2; S3.4: if the final queue is empty, but the map boundary is not encountered in the process of searching the pixel, it indicates that the occupied pixels form a closed space, which means that the map is closed at this time, stop updating the pixel map and save the map.

5. The grid map based brute-force matching loop closing method of claim 1, wherein: The step S4 specifically comprises: After detecting the initial two-dimensional plane pixel map constructed in S2 by S3, the initial point cloud map constructed in S2 is projected into the grid map to construct the initial matching grid map; the coverage grid map updates the maximum likelihood estimation through grid hit or miss, so as to achieve the effect of dynamically updating the point cloud map; after obtaining the superimposed classified point cloud, each frame of point cloud is input into the grid map as prior data: representing a current point cloud frame information, representing a coordinate and pose of the first point; To generate a grid map with the maximum probability of conforming to the current frame and historical frame information data: In the initial point cloud frame superposition process, the maximum likelihood estimation of each grid in the grid map is updated synchronously by formula (5), the prior point cloud data in formula (3) is updated, and the maximum grid probability map in formula (4) is constructed. After the construction is completed, it is used as the initial map of the brute force matching; All grid coordinates between the hit point and the origin are calculated, as formula (6): wherein, and represents the distance between the hit point and the origin, represents the coordinate of the hit point, then the sign of can be determined to determine the specific value of the grid coordinate , such as formula (7): 。 6. The grid map based brute-force matching loop closing method of claim 1, wherein: Two-dimensional pixel weighted projection is performed using point cloud information, and a two-dimensional plane is used for breadth-first search to detect environmental closure.

7. The grid map based brute-force matching loop closing method of claim 1, wherein: The point cloud information is projected to the grid map, and the optimized pose and laser information are used for grid map-based brute force matching to screen key frames and superposition window poses to ensure the accuracy of the positioning system, thereby realizing the loop detection optimization of the cumulative error.

Images