A fast loop detection method for autonomous driving positioning in mine environment based on lidar

Abstract

The present invention discloses a rapid loop detection method for autonomous driving positioning in a mine environment based on laser radar. The method first restricts the laser radar point cloud to a certain area and performs a straight-through filter. Then, the space is gridded from a bird's-eye view. The point cloud height is accumulated in each grid, and point clouds exceeding a certain threshold are retained. Secondly, the processed point cloud is partitioned into annular and fan-shaped areas to record the average height of the point cloud, and saved as a two-dimensional matrix to form a descriptor for a single-frame laser radar. The encoding value of each annular area is then used to construct a KD tree for rapid retrieval. The descriptor of the candidate frame is calculated for similarity with the descriptor of the current frame to obtain the best match to complete loop detection. The method uses the point cloud features of obstacles for loop detection. The present invention solves the problem of a large number of false positives in existing laser radar loop detection methods in mine scenes, and can achieve effective and accurate loop detection in scenes with tunnel tops and large slope changes. The method can be seamlessly integrated into the ROS system and can process point cloud data in real time after being accelerated by Numba, with high potential commercial value.

Description

Technical Field

[0001] The present invention relates to unmanned driving-related algorithms, point cloud processing, SLAM, embedded software and other technologies, and specifically to a fast loop detection method for autonomous driving positioning in a mine environment based on laser radar. Background Art

[0002] In the field of autonomous driving, SLAM (Simultaneous Localization and Mapping) primarily addresses the localization and mapping challenges faced by robots navigating unknown environments. Loop detection, one of the four major SLAM modules, detects potential loops and uses them to correct drift errors, reducing cumulative errors during SLAM mapping and improving map accuracy. If the tracking algorithm loses tracking in certain situations, loop detection can be used for relocalization. When the robot revisits a previously identified location, loop detection helps it accurately determine its position. Existing LiDAR loop detection algorithms are primarily targeted at large-scale outdoor environments. Some use the IMU's accumulated trajectory to determine whether it returns to the origin, while others use a partial scene descriptor based on the highest point in the environment. However, in mine tunnels, where space is confined, the roadway is rugged, and includes many steep slopes, the area above the tunnel is limited. Simply using the IMU and the highest point for loop detection can result in numerous loop detection errors, leading to inaccurate mapping. For this scenario, the present invention utilizes point cloud rasterization and quantitative statistics to accumulate heights, extracts environmental features from objects with accumulated heights, and then generates descriptors based on the average heights in annular and fan-shaped areas. This effectively improves the accuracy of loop detection in mine tunnel scenarios. Furthermore, the present invention utilizes Numba optimization to achieve high processing speed, meeting the requirements of keyframe loop detection. Its minimal performance overhead enables it to run on industrial computers with lower computing power, thus possessing enormous potential commercial value. Summary of the Invention

[0003] The purpose of the present invention is to provide a rapid loop detection method for autonomous driving positioning in a mine environment based on lidar, so as to improve the false positives of traditional loop detection and improve the loop accuracy in a mine environment with a top surface and large slope changes.

[0004] The technical solution to achieve the purpose of the present invention is: a rapid loop detection method for autonomous driving positioning in a mine environment based on laser radar, comprising the following steps:

[0005] Step 1: Subscribe to the topic of the lidar point cloud data in the ROS operating system, pass the point cloud data back to the callback_point function to limit the area of ​​the lidar point cloud data, use list() to accelerate reading when reading the point cloud, and assign the point cloud data to the points variable;

[0006] Step 2: Assign the data in the points variable to the voxel map according to the XYZ position, rasterize the lidar data, and then use the index assigned by rasterization to count the point cloud data. Discard or retain the point cloud data according to the statistical threshold, and store the retained point cloud data in the p_result list;

[0007] Step 3: Use the Numba JIT optimized decorator to accelerate the point cloud traversal and publish the processed point cloud data using the handle in rospy. The published topic is / rslidar_points / points_fusion_remove_ground.

[0008] Step 4: Subscribe to the lidar point cloud data named / rslidar_points / points_fusion_remove_ground, decompose the point cloud into 10 rings using polar coordinates from a bird's-eye view, and decompose each ring into 30 sectors. In each sector, take the average height of the point cloud and save it into a 10*30 two-dimensional matrix to form the loop detection descriptor desc_matrix, and generate the ring key ring_key based on desc_matrix.

[0009] Step 5: Build a kd tree based on the ring key ring_key in the descriptor, use the kd tree to find the candidate desc_matrix, and then calculate the similarity of desc_matrix. The similarity between the descriptor of the candidate frame and the descriptor of the current frame is judged by the threshold. If it exceeds the threshold, it is considered a valid loop.

[0010] Furthermore, in step 1, subscribe to the topic of the lidar point cloud data in the ros operating system, return the point cloud data to the callback_point function to perform regional restrictions on the lidar point cloud data, use list() to accelerate reading when reading the point cloud, and assign the point cloud data to the points variable. The specific method is:

[0011] In the ros system, the rospy.Subscriber function is used to subscribe to the point cloud data topic of the lidar. / rslidar_points is used as the topic of the point cloud data. The callback function of rospy.Subscriber is defined as callback_point. In callback_point, the point cloud library point_cloud2 in the ros system and the read_points method in the library are used to read the point cloud data. np.array(list(point_cloud2.read_points(msg))).astype(np.float32) is used to convert the point cloud data into np.array type, where msg is the variable for storing lidar data. np.abs is used to obtain the absolute value of XYZ in the lidar point cloud data. According to the preset X max 、X min 、Y max 、Y min Compare the absolute values ​​of the XYZ values ​​of the lidar data, filter out points outside the area, form a rectangular space from a bird's-eye view, and finally store the data in the points variable.

[0012] Furthermore, in step 2, the data in the points variable is assigned to the voxel map according to the XYZ position, the lidar data is rasterized, and the point cloud data is counted using the index assigned by rasterization. The point cloud data is discarded or retained according to the statistical threshold, and the retained point cloud data is stored in the p_result list. The specific method is:

[0013] Define a function named remove_ground_by_grid_diff_numba_helper that accepts a np.array type data named target_np_noised as input, which is the points variable in step 1.

[0014] The process of creating a voxel map of a preset size S, where r represents the resolution, can be described as follows:

[0015]

[0016] Initialize the voxel map. Mmin and Mmax are used to record the minimum and maximum z values ​​of the point cloud in each voxel when traversing the point cloud, that is, the point cloud height value of the lidar. N is used to record the number of point clouds in each voxel. The Mmin matrix is ​​initialized to all values ​​of -10000, the Mmax matrix is ​​initialized to all values ​​of 10000, and N is initialized to 0. The process can be described as follows:

[0017] Mmin=ones(S[1],S[0])×10000

[0018] Mmax=ones(S[1],S[0])×-10000

[0019] N=zsros(S[1],S[0])

[0020] Where S[1] and S[0] are the length and width of the voxel map respectively, N is a matrix that records the number of point clouds in each voxel, ones(.,.) represents a two-dimensional matrix of all 1s, and zeros(.,.) represents a two-dimensional matrix of all 0s;

[0021] Update the voxel map, traverse the input point cloud, and for each point p, where p[0] is the x coordinate of the current point, p[1] is the y coordinate of the current point, and p[2] is the z coordinate of the current point, calculate the coordinates of each point in the voxel map (c p_x ,c p_y ), and update the coordinates in the voxel map (c x ,c y ), if the point is within the voxel map range, update Mmin, Mmax and N. The process is described as follows:

[0022]

[0023]

[0024] N[c p_y ,c p_x ]+=1

[0025] Mmin[c y ,c x ]=min(Mmin[c p_y ,c p_x ],p[2])

[0026] Mmax[c y ,c x ]=max(Mmax[c p_y ,c p_x ],p[2])

[0027] For each point, check whether its count value in the voxel map is less than or equal to count_num, count_num is set to 5, if the count value is less than count_num, then mark the point as a ground point, otherwise add it to the p_result list, and finally return the p_result list.

[0028] Furthermore, in step 3, the Numba JIT optimized decorator is used to accelerate the point cloud traversal, and the processed point cloud data is published using the handle in rospy. The published topic is / rslidar_points / points_fusion_remove_ground. The specific method is:

[0029] Load the numba library function and use the @numba.jit(nopython=True,cache=True) statement before the remove_ground_by_grid_diff_numba_helper function to accelerate traversal. Set nopython to True to instruct Numba to try to generate code that does not require a Python interpreter to achieve optimal performance. Set cache to True to instruct Numba to cache the compiled function on disk to speed up the program startup time.

[0030] Read the p_result list into the msg variable. The timestamp of the msg variable uses the current time. Use rospy.Publisher(" / rslidar_points / points_fusion_remove_ground",PointCloud2,queue_size=3) and publish(msg) to publish the lidar point cloud data named / rslidar_points / points_fusion_remove_ground. PointCloud2 means that the data will be published in PointCloud2 format. queue_size is the length of the message queue.

[0031] Furthermore, in step 4, subscribe to the lidar point cloud data named / rslidar_points / points_fusion_remove_ground, decompose the point cloud into 10 rings using polar coordinates from a bird's-eye view, and decompose 30 sectors in each ring. In each sector, take the average height of the point cloud and save it in a 10*30 two-dimensional matrix to form a loop detection descriptor desc_matrix, and generate a ring key ring_key based on desc_matrix. The specific method is as follows:

[0032] Using nh.subscribe<sensor_msgs::PointCloud2> The statement subscribes to the topic / rslidar_points / points_fusion_remove_ground. In the callback function, a matrix desc_matrix with a size of 10 rows and 30 columns is initialized, and all its elements are initialized to -1000 to store the generated SCdes descriptor.

[0033] Traverse the point cloud of the current frame, save the coordinates (x, y, z) of the current point in the pt variable, and add the z coordinate to the height of the laser radar LIDAR_HEIGHT to get the relative height of the point on the ground;

[0034] Convert the Cartesian coordinates of the point to polar coordinates, and calculate the polar distance Polar_coordinate_distance and polar angle Polar_coordinate_angle of the point; the calculation method is as follows:

[0035]

[0036]

[0037] According to the polar coordinate distance and angle of the point, calculate its row and column index ring_idx and sector_idx in the descriptor matrix desc,

[0038]

[0039]

[0040] In the above formula, NUM_RING indicates the number of ring segments, 10 rings are used, MAX_RADIUS indicates the maximum processing distance, which is set to 20m, and NUM_SECTOR indicates the number of sector segments, 30 sectors are used.

[0041] Accumulate and calculate the average height of the values ​​corresponding to the row and column indexes of the current descriptor matrix, store them in pt.z, retain the average height value of the position corresponding to the row and column indexes, and return the generated descriptor matrix desc_matrix;

[0042] Generate a corresponding ring key according to each row of desc_matrix. The specific method of ring key encoding is:

[0043]

[0044]

[0045] ring_key is a one-dimensional array that represents the ring key in the current descriptor. It is used to store the average value of each row of descriptors. By looping through each row of the descriptor matrix desc_matrix, the average value of each row of descriptors is calculated and the result is stored in the corresponding position of the feature vector matrix ring_key. i It represents the i-th ring, that is, the row index in desc_matrix, where ||r i ||0 means r i The number of non-zeros in , N s is the number of fan-shaped areas, take 30, N r is the number of annular regions, set to 10.

[0046] Furthermore, in step 5, a kd tree is constructed based on the ring key ring_key in the descriptor, and the kd tree is used to find the candidate desc_matrix. Then, the similarity of the desc_matrix is ​​calculated. The similarity between the descriptor of the candidate frame and the descriptor of the current frame is judged by a threshold. If the similarity exceeds the threshold, it is considered a valid loop. The specific method is as follows:

[0047] Use the nanoflann library to perform k-nearest neighbor search. Declare a variable search_result of type KNNResultSet to store the results of the k-nearest neighbor search. Set NUM_CANDIDATES as the number of nearest neighbor candidate points returned to 3. Call the init() function to initialize the internal data structure of the search result set. Call the findNeighbors() function to perform the k-nearest neighbor search. The pointer to the current query key point in the findNeighbors() function is assigned to the ring_key obtained in step 4. After the search is completed, read the desc_matrix of the three searched candidate frames from search_result and calculate the similarity with the desc_matrix descriptor of the current frame. The calculation formula is as follows:

[0048]

[0049] I 0 ,I 1 Represent the descriptor of the current frame and the descriptor of the candidate frame respectively, Represents each column vector of the current frame descriptor and the candidate frame descriptor, and is calculated using the cosine distance The total distance is averaged to obtain the similarity between the two descriptors. When the cosine distance is less than THRES, a loop is determined to occur, and the current frame index cur_idx and the frame index can_idx of the candidate frame are returned to complete the loop detection.

[0050] A rapid loopback detection system for autonomous driving positioning in a mine environment based on a laser radar implements the rapid loopback detection method for autonomous driving positioning in a mine environment based on a laser radar to achieve rapid loopback detection for autonomous driving positioning in a mine environment based on a laser radar.

[0051] A computer device includes a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the method for rapid loopback detection of autonomous driving positioning in a mine environment based on a laser radar is implemented to achieve rapid loopback detection of autonomous driving positioning in a mine environment based on a laser radar.

[0052] A computer-readable storage medium stores a computer program. When the computer program is executed by a processor, the method for rapid loopback detection of autonomous driving positioning in a mine environment based on a laser radar is implemented to achieve rapid loopback detection of autonomous driving positioning in a mine environment based on a laser radar.

[0053] Compared with existing technologies, this invention has significant advantages: significantly improved accuracy compared to existing highest-point-based lidar loop detection, fast search speed, and the use of Numba acceleration, which enables excellent real-time performance. Furthermore, it is developed based on the ROS system and has strong compatibility. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 A fast loop detection method for autonomous driving positioning in a mine environment based on lidar.

[0055] Figure 2 The laser radar bird's-eye view is divided into ring and sector-shaped areas. The intersection of light yellow and light blue is the intersection of the fourth ring and the third sector.

[0056] Figure 3 It is a global algorithm framework for unmanned driving.

[0057] Figure 4 This is the loop detected by the traditional algorithm in a highly restricted environment. The yellow line represents the loop constraint.

[0058] Figure 5 This is the loop detected by the present invention in a highly restricted environment. The yellow line represents the loop constraint. DETAILED DESCRIPTION

[0059] In order to make the purpose, technical solutions and advantages of this application more clear, the following further describes this application in detail with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain this application and are not intended to limit this application.

[0060] The present invention rasterizes the original point cloud, performs regional height accumulation statistics, uses the processed point cloud to construct a loop detection descriptor, then uses the ring key to construct a KD tree to search the nearest neighbor to obtain the candidate frame index, calculates the similarity between the current frame and the candidate frame, and determines it as a valid loop if it exceeds the threshold.

[0061] A rapid loop detection method for autonomous driving positioning in a mine environment based on laser radar includes the following steps:

[0062] Step 1: Subscribe to the topic of the lidar point cloud data in the ROS operating system, pass the point cloud data back to the callback_point function to limit the area of ​​the lidar point cloud data, use list() to accelerate reading when reading the point cloud, and assign the point cloud data to the points variable;

[0063] In the ros system, the rospy.Subscriber function is used to subscribe to the point cloud data topic of the lidar. In the present invention, / rslidar_points is used as the topic of the point cloud data. The callback function of rospy.Subscriber is defined as callback_point. In callback_point, np.array(list(point_cloud2.read_points(msg))).astype(np.float32) is used to read the point cloud data and convert it into np.array type. According to engineering experience, adding the list() statement can significantly increase the speed of reading point cloud data. The absolute value of XYZ in the lidar point cloud data is obtained by np.abs. According to the preset X max 、X min 、Y max 、Y min Compare the absolute values ​​of the laser radar data XYZ, filter out the points outside the area, and form a rectangular space from a bird's-eye view. max Set to 50, X min Set to -50, Y max Set to 20, Y min Set it to -20 and finally store the data in the points variable.

[0064] Step 2: Assign the data in the points variable to the voxel map according to the XYZ position, rasterize the lidar data, and then use the index during rasterization to count the point cloud data. Set the statistical threshold count_num, discard or retain the point cloud data according to the statistical threshold, and store the point cloud data in the p_result list.

[0065] Define a function named remove_ground_by_grid_diff_numba_helper that receives an np.array type data named target_np_noised as input, which is the points variable in step 1.

[0066] Initialize the voxel map. Mmin and Mmax are initialized to very large and very small values, respectively. These are used to record the minimum and maximum z values ​​(i.e., the height of the LiDAR point cloud) of each voxel as the point cloud is traversed. N is used to record the number of points in each voxel. For example, the Mmin matrix is ​​set to all -10,000 values, and the Mmax matrix is ​​set to all 10,000 values. N is set to 0. The process can be described as follows:

[0067] Mmin=ones(S[1],S[0])×10000

[0068] Mmax=ones(S[1],S[0])×-10000

[0069] N=zsros(S[1],S[0])

[0070] S[1] and S[0] are the length and width of the voxel map respectively, N is a matrix that records the number of point clouds in each voxel, ones(.,.) represents a two-dimensional matrix of all 1s, and zeros(.,.) represents a two-dimensional matrix of all 0s.

[0071] Update the voxel map, traverse the input point cloud, and for each point p, where p[0] is the x coordinate of the current point, p[1] is the y coordinate of the current point, and p[2] is the z coordinate of the current point, calculate the coordinates of each point in the voxel map (c p_x ,c p_y ), and update the coordinates in the voxel map (c x ,c y ), if the point is within the voxel map range, update Mmin, Mmax and N. The process is described as follows:

[0072]

[0073]

[0074] N[c p_y ,c p_x ]+=1

[0075] Mmin[c y ,c x ]=min(Mmin[c p_y ,c p_x ],p[2])

[0076] Mmax[c y ,c x ]=max(Mmax[c p_y ,c p_x ],p[2])

[0077] For each point, check whether its count value in the voxel map is less than or equal to count_num. Based on engineering experience, count_num can be set to 5. If the count value is less than count_num, mark the point as a ground point. Otherwise, add it to the p_result list. Finally, return the p_result list.

[0078] Step 3: Use the Numba JIT (Just-in-Time Compilation) optimized decorator on the function in step 2 to accelerate the point cloud traversal; and use the handle in rospy to publish the processed point cloud data. The topic published in this invention is / rslidar_points / points_fusion_remove_ground;

[0079] The specific steps for using Numba to JIT (just-in-time compilation) optimize the decorator of the function in step 2 and publish the point cloud data are as follows:

[0080] Import numba at the top of the file to load the numba library. Use the @numba.jit(nopython=True,cache=True) statement before the remove_ground_by_grid_diff_numba_helper function to accelerate traversal. Setting nopython to True instructs Numba to generate code that doesn't require a Python interpreter for optimal performance. Setting cache to True instructs Numba to cache compiled functions on disk, speeding up program startup time. Read the p_result list into the msg variable, using the current time as the timestamp. Use rospy.Publisher(" / rslidar_points / points_fusion_remove_ground",PointCloud2,queue_size=3) and publish(msg) to publish the lidar point cloud data named / rslidar_points / points_fusion_remove_ground.

[0081] Step 4: Subscribe to the lidar point cloud data named / rslidar_points / points_fusion_remove_ground. Decompose the point cloud into 10 rings using polar coordinates from a bird's-eye view. Decompose each ring into 30 sectors. Take the average height of the point cloud in each sector and save it into a 10*30 two-dimensional matrix to form the loop detection descriptor desc_matrix. Generate the ring key ring_key based on desc_matrix.

[0082] Using nh.subscribe<sensor_msgs::PointCloud2> The statement subscribes to the topic / rslidar_points / points_fusion_remove_ground. In the callback function, a matrix desc_matrix with 10 rows and 30 columns is initialized, and all its elements are initialized to -1000 to store the generated SCdes descriptor. The current frame point cloud is traversed, the coordinates (x, y, z) of the current point are saved in the pt variable, and the z coordinate is added to the lidar height LIDAR_HEIGHT to obtain the point's relative height to the ground. The Cartesian coordinates of the point are converted to polar coordinates, and the polar distance Polar_coordinate_distance and polar angle Polar_coordinate_angle of the point are calculated. Based on the polar distance and angle of the point, its row and column indices ring_idx and sector_idx in the descriptor matrix desc are calculated. The values ​​corresponding to the row and column indices of the current descriptor matrix are accumulated and statistically calculated to obtain the average height, which is stored in pt.z. The average height value corresponding to the row and column indices is retained. The generated descriptor matrix desc_matrix is ​​returned; a corresponding ring key is generated for each row of desc_matrix. The specific method for encoding the ring key is as follows:

[0083]

[0084]

[0085] ring_key is a one-dimensional array representing the ring key in the current descriptor, where r i It represents the i-th ring, that is, the row index in desc_matrix, where ||r i ||0 means r i The number of non-zeros in , N s is the number of fan-shaped areas, take 30, N r is the number of annular regions, which is 10;

[0086] The implementation logic of the code is as follows: construct an eigenvector matrix ring_key with the same size as the number of rows of the descriptor matrix desc_matrix and 1 column to store the average value of the descriptor in each row. By looping through each row of the descriptor matrix desc_matrix, calculate the average value of the descriptor in each row and store the result in the corresponding position of the eigenvector matrix ring_key.

[0087] Step 5: Build a kd tree based on the ring key ring_key in the descriptor, use the kd tree to find the candidate desc_matrix, and then calculate the similarity of desc_matrix. The similarity between the descriptor of the candidate frame and the descriptor of the current frame is judged by the threshold. If it exceeds the threshold, it is considered a valid loop.

[0088] Use the nanoflann library to perform k-nearest neighbor search and declare a variable search_result of type KNNResultSet to store the results of the k-nearest neighbor search. Set NUM_CANDIDATES as the number of nearest neighbor candidate points returned. In this invention, NUM_CANDIDATES is set to 3. Call the init() function to initialize the internal data structure of the search result set, call the findNeighbors() function to perform the k-nearest neighbor search, and assign the pointer to the current query key point in the findNeighbors() function to the ring_key obtained in step 4. After the search is completed, read the desc_matrix of the three searched candidate frames from search_result and calculate the similarity with the desc_matrix descriptor of the current frame. The calculation formula is as follows:

[0089]

[0090] I 0 ,I 1 Represent the descriptor of the current frame and the descriptor of the candidate frame respectively, Represents each column vector of the current frame descriptor and the candidate frame descriptor, and is calculated using the cosine distance The total distance is averaged to obtain the similarity between the two descriptors. When the cosine distance is less than THRES (THRES is set to 0.3 in this invention), a loop is determined to have occurred, and the current frame index cur_idx and the candidate frame index can_idx are returned. After loop detection is completed, the SLAM backend is optimized to calibrate the accumulated error.

[0091] The present invention is further analyzed and described in detail in one embodiment. In this embodiment, an autonomous driving system is first deployed using an underground experimental vehicle, and the effectiveness of the loop detection method is verified by the actual performance of the vehicle in test scenarios such as ramps and obstacle avoidance.

[0092] Example

[0093] In order to verify the effectiveness of the solution of the present invention, the following experiment was conducted.

[0094] (1) Experimental preparation

[0095] The experimental vehicle hardware includes an NVIDIA Jetson Orin 64G*, an Ouster OS1-64 laser radar*, and a Livox Mid360 laser radar*. The software is deployed using Ubuntu 20.04 and ROS Noetic. LIO-SAM is used for mapping and localization. The navigation module is based on the Move Base, using Dijkstra as the global path planner and Teb Local Planner as the local path planner.

[0096] (2) Topology of experimental platform hardware and software modules

[0097] The present invention is based on the ROS system. As a functional module in ROS, it subscribes to the original point cloud information and outputs the point cloud after removing the ground. The output point cloud data is connected to the obstacle avoidance and path planning module for use by subsequent modules. Figure 3 The topology diagram of the experimental platform's hardware and software modules. The dotted box represents the loop detection method described in the present invention.

[0098] (3) Tunnel experiment

[0099] The method of the present invention is used to perform loop detection during the site mapping process. Figure 4 Traditional loop detection results in many false detections. Figure 5 This is the loop detection after using the present invention. The blue line segment is the path that the unmanned vehicle passes through, and the yellow line segment represents the constraints of the loop detection.

[0100] (4) Analysis of experimental results

[0101] When the present invention is not used, traditional loop closure detection will produce many false detections and has low accuracy. After using the method proposed in the present invention, the object features in space can be effectively utilized, the accuracy of the loop closure is improved, and the accuracy of the loop closure detection is improved.

[0102] The technical features of the above embodiments can be combined arbitrarily. To make the description concise, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

[0103] The above-described embodiments merely represent several implementation methods of the present application. While the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of the present application. It should be noted that a person of ordinary skill in the art may make various modifications and improvements without departing from the spirit of the present application, and these modifications and improvements fall within the scope of protection of the present application. Therefore, the scope of protection of the present application shall be determined by the appended claims.

Claims

1. A fast loop detection method for autonomous driving positioning in a mine environment based on laser radar, characterized in that: The following steps are involved: Step 1: Subscribe to the topic of the lidar point cloud data in the ROS operating system, pass the point cloud data back to the callback_point function to limit the area of ​​the lidar point cloud data, use list() to accelerate reading when reading the point cloud, and assign the point cloud data to the points variable; Step 2: Assign the data in the points variable to the voxel map according to the XYZ position, rasterize the lidar data, and then use the index assigned by rasterization to count the point cloud data. Discard or retain the point cloud data according to the statistical threshold, and store the retained point cloud data in the p_result list; Step 3: Use the Numba JIT optimized decorator to accelerate the point cloud traversal and publish the processed point cloud data using the handle in rospy. The published topic is / rslidar_points / points_fusion_remove_ground. Step 4: Subscribe to the lidar point cloud data named / rslidar_points / points_fusion_remove_ground, decompose the point cloud into 10 rings using polar coordinates from a bird's-eye view, and decompose each ring into 30 sectors. In each sector, take the average height of the point cloud and save it into a 10*30 two-dimensional matrix to form the loop detection descriptor desc_matrix, and generate the ring key ring_key based on desc_matrix. Step 5: Build a kd tree based on the ring key ring_key in the descriptor, use the kd tree to find the candidate desc_matrix, and then calculate the similarity of desc_matrix. The similarity between the descriptor of the candidate frame and the descriptor of the current frame is judged by the threshold. If it exceeds the threshold, it is considered a valid loop.

2. The rapid loop detection method for autonomous driving positioning in a mine environment based on laser radar according to claim 1 is characterized in that: Step 1: Subscribe to the topic of the lidar point cloud data in the ROS operating system, pass the point cloud data back to the callback_point function to perform regional restrictions on the lidar point cloud data, use list() to accelerate reading when reading the point cloud, and assign the point cloud data to the points variable. The specific method is: In the ros system, the rospy.Subscriber function is used to subscribe to the point cloud data topic of the lidar. / rslidar_points is used as the topic of the point cloud data. The callback function of rospy.Subscriber is defined as callback_point. In callback_point, the point cloud library point_cloud2 in the ros system and the read_points method in the library are used to read the point cloud data. np.array(list(point_cloud2.read_points(msg))).astype(np.float32) is used to convert the point cloud data into np.array type, where msg is the variable for storing lidar data. np.abs is used to obtain the absolute value of XYZ in the lidar point cloud data. According to the preset X max 、X min 、Y max 、Y min Compare the absolute values ​​of the XYZ values ​​of the lidar data, filter out points outside the area, form a rectangular space from a bird's-eye view, and finally store the data in the points variable.

3. The rapid loop detection method for autonomous driving positioning in a mine environment based on laser radar according to claim 2 is characterized in that: Step 2: Assign the data in the points variable to the voxel map according to the XYZ position, rasterize the lidar data, and then use the index assigned by rasterization to count the point cloud data. Discard or retain the point cloud data according to the statistical threshold, and store the retained point cloud data in the p_result list. The specific method is: Define a function named remove_ground_by_grid_diff_numba_helper that accepts a np.array type data named target_np_noised as input, which is the points variable in step 1. The process of creating a voxel map of a preset size S, where r represents the resolution, can be described as follows: Initialize the voxel map. Mmin and Mmax are used to record the minimum and maximum z values ​​of the point cloud in each voxel when traversing the point cloud, that is, the point cloud height value of the lidar. N is used to record the number of point clouds in each voxel. The Mmin matrix is ​​initialized to all values ​​of -10000, the Mmax matrix is ​​initialized to all values ​​of 10000, and N is initialized to 0. The process can be described as follows: Mmin=ones(S[1],S[0])×10000 Mmax=ones(S[1],S[0])×-10000 N=zsros(S[1],S[0]) Where S[1] and S[0] are the length and width of the voxel map respectively, N is a matrix that records the number of point clouds in each voxel, ones(.,.) represents a two-dimensional matrix of all 1s, and zeros(.,.) represents a two-dimensional matrix of all 0s; Update the voxel map, traverse the input point cloud, and for each point p, where p[0] is the x coordinate of the current point, p[1] is the y coordinate of the current point, and p[2] is the z coordinate of the current point, calculate the coordinates of each point in the voxel map (c p_x ,c p_y ), and update the coordinates in the voxel map (c x ,c y ), if the point is within the voxel map range, update Mmin, Mmax and N. The process is described as follows: N[c p_y ,c p_x ]+=1 Min[c y ,c x ]=min(Mmin[c p_y ,c p_c ],p[2]) Mmax[c y ,c x ]=max(Mmax[c p_y ,c p_x ],p[2]) For each point, check whether its count value in the voxel map is less than or equal to count_num, count_num is set to 5, if the count value is less than count_num, then mark the point as a ground point, otherwise add it to the p_result list, and finally return the p_result list.

4. The rapid loop detection method for autonomous driving positioning in a mine environment based on laser radar according to claim 3 is characterized in that: Step 3: Use the Numba JIT optimized decorator to accelerate the point cloud traversal and use the handle in rospy to publish the processed point cloud data. The published topic is / rslidar_points / points_fusion_remove_ground. The specific method is: Load the numba library function and use the @numba.jit(nopython=True,cache=True) statement before the remove_ground_by_grid_diff_numba_helper function to accelerate traversal. Set nopython to True to instruct Numba to try to generate code that does not require a Python interpreter to achieve optimal performance. Set cache to True to instruct Numba to cache the compiled function on disk to speed up the program startup time. Read the p_result list into the msg variable. The timestamp of the msg variable uses the current time. Use rospy.Publisher(" / rslidar_points / points_fusion_remove_ground",PointCloud2,queue_size=3) and publish(msg) to publish the lidar point cloud data named / rslidar_points / points_fusion_remove_ground. PointCloud2 means that the data will be published in PointCloud2 format. queue_size is the length of the message queue.

5. The rapid loop detection method for autonomous driving positioning in a mine environment based on laser radar according to claim 1 is characterized in that: Step 4: Subscribe to the lidar point cloud data named / rslidar_points / points_fusion_remove_ground. Decompose the point cloud into 10 rings using polar coordinates from a bird's-eye view. Decompose each ring into 30 sectors. Take the average height of the point cloud in each sector and save it into a 10*30 two-dimensional matrix to form the loop detection descriptor desc_matrix. Generate the ring key ring_key based on desc_matrix. The specific method is as follows: Using nh.subscribe<sensor_msgs::PointCloud2> The statement subscribes to the topic / rslidar_points / points_fusion_remove_ground. In the callback function, a matrix desc_matrix with a size of 10 rows and 30 columns is initialized, and all its elements are initialized to -1000 to store the generated SCdes descriptor. Traverse the point cloud of the current frame, save the coordinates (x, y, z) of the current point in the pt variable, and add the z coordinate to the height of the laser radar LIDAR_HEIGHT to get the relative height of the point on the ground; Convert the Cartesian coordinates of the point to polar coordinates, and calculate the polar distance Polar_coordinate_distance and polar angle Polar_coordinate_angle of the point; the calculation method is as follows: According to the polar coordinate distance and angle of the point, calculate its row and column index ring_idx and sector_idx in the descriptor matrix desc, In the above formula, NUM_RING indicates the number of ring segments, 10 rings are used, MAX_RADIUS indicates the maximum processing distance, which is set to 20m, and NUM_SECTOR indicates the number of sector segments, 30 sectors are used. Accumulate and calculate the average height of the values ​​corresponding to the row and column indexes of the current descriptor matrix, store them in pt.z, retain the average height value of the position corresponding to the row and column indexes, and return the generated descriptor matrix desc_matrix; Generate a corresponding ring key according to each row of desc_matrix. The specific method of ring key encoding is: ring_key is a one-dimensional array representing the ring key in the current descriptor, used to store the average value of each row of descriptors, r i It represents the i-th ring, that is, the row index in desc_matrix, where ||r i ||0 means r i The number of non-zeros in , N s is the number of fan-shaped areas, take 30, N r is the number of annular regions, set to 10.

6. The rapid loop detection method for autonomous driving positioning in a mine environment based on laser radar according to claim 5 is characterized in that: Step 5: Build a kd tree based on the ring key ring_key in the descriptor, use the kd tree to find the candidate desc_matrix, and then calculate the similarity of desc_matrix. The similarity between the descriptor of the candidate frame and the descriptor of the current frame is judged by the threshold. If it exceeds the threshold, it is considered a valid loop. The specific method is: Use the nanoflann library to perform k-nearest neighbor search. Declare a variable search_result of type KNNResultSet to store the results of the k-nearest neighbor search. Set NUM_CANDIDATES as the number of nearest neighbor candidate points returned to 3. Call the init() function to initialize the internal data structure of the search result set. Call the findNeighbors() function to perform the k-nearest neighbor search. The pointer to the current query key point in the findNeighbors() function is assigned to the ring_key obtained in step 4. After the search is completed, read the desc_matrix of the three searched candidate frames from search_result and calculate the similarity with the desc_matrix descriptor of the current frame. The calculation formula is as follows: I 0 ,I 1 Represent the descriptor of the current frame and the descriptor of the candidate frame respectively, Represents each column vector of the current frame descriptor and the candidate frame descriptor, and is calculated using the cosine distance The total distance is averaged to obtain the similarity between the two descriptors. When the cosine distance is less than THRES, a loop is determined to occur, and the current frame index cur_idx and the frame index can_idx of the candidate frame are returned to complete the loop detection.

7. A fast loop detection system for autonomous driving positioning in a mine environment using a laser radar, characterized in that: Implement the rapid loop detection method for autonomous driving positioning in a mine environment based on lidar as described in any one of claims 1-6 to realize rapid loop detection of autonomous driving positioning in a mine environment based on lidar.

8. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the method for rapid loopback detection of autonomous driving positioning in a mine environment based on a laser radar as described in any one of claims 1 to 6 is implemented to realize rapid loopback detection of autonomous driving positioning in a mine environment based on a laser radar.

9. A computer-readable storage medium having a computer program stored thereon. When the computer program is executed by a processor, the method for rapid loop detection of autonomous driving positioning in a mine environment based on a laser radar as described in any one of claims 1 to 6 is implemented to realize rapid loop detection of autonomous driving positioning in a mine environment based on a laser radar.

Images