Method, System and Device for Point Cloud Semantic SLAM Optimized Based on Human-in-the-Loop

Abstract

The present invention discloses a method, system and device for robot point cloud semantic SLAM optimized based on human-in-the-loop. Among them, the method includes: S1. The lidar on the mobile robot collects data from the target location to obtain the relative pose information of each frame of point cloud and complete mapping; S2. Establish a continuous trajectory of the point cloud, manually observe the mapping error, construct a dense factor graph, and then perform global optimization based on the factor graph to adjust the overall point cloud error; S3. Based on the predefined point cloud semantic categories and using annotation tools to perform point cloud semantic annotation of the superimposed map, and accumulate the point cloud semantic labeling results to obtain a point cloud semantic segmentation data set; S4. Train and evaluate the segmentation model based on the point cloud semantic segmentation data set to obtain the trained and evaluated segmentation model; S5. Based on the trained and evaluated segmentation model, perform point cloud semantic localization and optimization. Using the present invention, point cloud semantic localization and optimization can be achieved.

Description

Technical Field

[0001] The present invention relates to the field of optimizing point cloud semantic SLAM, and in particular to a method, system and device for point cloud semantic SLAM based on human-in-the-loop optimization. Background Art

[0002] With the rapid development of technology, the related applications of mobile robots are gradually entering daily life from the research stage. Especially during the fight against Covid-19, mobile robots have undertaken a series of contactless tasks such as material distribution, disinfection, and cleaning services.

[0003] The autonomous perception and positioning of mobile robots in the environment are the most basic functions to ensure downstream tasks. Currently, the mainstream robot autonomous positioning technologies include simultaneous localization and mapping (SLAM), Ultra-Wide Bandwith (UWB), wheel speedometers, GPS, and multi-sensor fusion solutions, etc. Among them, GPS fails in occluded environments or indoor positioning; UWB requires additional construction deployment and a relatively high device density; wheel speedometers are vulnerable to ground slippage and wheel deformation, and also require dynamic modeling of the robot chassis, and the estimation accuracy of the rotation attitude is also poor. The SLAM algorithm based on open-loop control has less dependence on equipment and has broader stability and adaptability to applicable scenarios.

[0004] The existing SLAM algorithms can be roughly divided into two categories: image-based and point cloud SLAM. The lidar based on active perception is not affected by ambient light and can be used normally in a dark environment, which provides a reliable guarantee for the use of mobile robots in extremely harsh lighting environments. The SLAM technology based on LiDAR can provide positioning information and a stable and high-fidelity dense map point cloud of the environment for mobile robots, and is one of the key modules for mobile robot application deployment. However, the sparsity, randomness, and unstable data structure of lidar point clouds make the difficulty of registering feature screening and matching more complex than that of images, which will lead to complex calculations and introduce cumulative errors, resulting in the degradation of the point cloud map and a map with poor global consistency. Thus, it affects the development of a series of downstream application tasks based on the point cloud map.

[0005] Although the prior art has carried out a series of optimizations on the front end and the back end of the SLAM system, the overall solution mainly relies on low-dimensional geometric features such as points, lines, and planes, lacking a human-like cognitive mode for understanding environmental semantic information. In addition, annotating the low-cost lidar point cloud data with sparse points is extremely challenging, and the semantic SLAM optimization scheme is also limited by the dataset support of the corresponding scenario, making the development of technologies including deep learning modeling and semantic SLAM optimization very slow. Summary of the Invention

[0006] The object of the present invention is to provide a method, system and device for point cloud semantic SLAM optimized based on human-in-the-loop, aiming to solve the optimization of point cloud semantic SLAM.

[0007] The present invention provides a method for point cloud semantic SLAM optimized based on human-in-the-loop, including:

[0008] S1. Use the acquisition module on the mobile robot to collect data from the target location. During the collection process, use the SLAM algorithm as the initial mapping algorithm to calculate the relative pose information of each frame of point cloud and complete the mapping;

[0009] S2. Establish a continuous trajectory of the point cloud, obtain the mapping error information observed manually, select the correct point cloud frames around the mapping error based on the mapping error information to re-perform the registration optimization of local ICP, obtain the closed loop between key pose points, construct a dense factor graph, and then perform global optimization based on the dense factor graph to adjust the overall mapping error, and iterate the registration optimization and global optimization;

[0010] S3. Import the point cloud poses after local ICP registration optimization and the corresponding point clouds of the optimized point cloud poses into the annotation tool, and use the annotation tool to perform point cloud semantic annotation of the superimposed map based on the predefined point cloud semantic categories, and accumulate the point cloud semantic marking results to obtain a point cloud semantic segmentation data set;

[0011] S4. Train and evaluate the point cloud semantic segmentation model based on the point cloud semantic segmentation data set to obtain the trained and evaluated point cloud semantic segmentation model;

[0012] S5. Perform point cloud semantic localization and optimization based on the trained and evaluated segmentation model.

[0013] The present invention also provides a system for point cloud semantic SLAM optimized based on human-in-the-loop, including:

[0014] An acquisition module, installed on the mobile robot, for collecting data from the target location. During the collection process, use the SLAM algorithm as the initial mapping algorithm to calculate the relative pose information of each frame of point cloud and complete the mapping;

[0015] An optimization module, for establishing a continuous trajectory, obtaining the mapping error information observed manually, selecting the correct point cloud frames around the mapping error based on the mapping error information to re-perform the registration optimization of local ICP, obtaining the closed loop between key pose points, constructing a dense factor graph, and then performing global optimization based on the dense factor graph to adjust the overall mapping error, and iterating the registration optimization and global optimization;

[0016] A dataset module, which is used to import the point cloud poses after local ICP registration optimization and the corresponding point clouds of the optimized point cloud poses into a labeling tool, and based on predefined point cloud semantic categories, use the labeling tool to perform point cloud semantic labeling of the overlay map, and accumulate the point cloud semantic labeling results to obtain a point cloud semantic segmentation dataset;

[0017] A segmentation model module, which is used to train and evaluate a segmentation model based on the point cloud semantic segmentation dataset to obtain a trained and evaluated segmentation model;

[0018] A positioning optimization module, which is used to perform point cloud semantic positioning and optimization based on the trained and evaluated segmentation model.

[0019] An embodiment of the present invention also provides a device for point cloud semantic SLAM based on human-in-the-loop optimization, including: a memory, a processor, and a computer program stored on the memory and executable on the processor. When the computer program is executed by the processor, the steps of the above method are implemented.

[0020] An embodiment of the present invention also provides a computer-readable storage medium, on which an implementation program for information transmission is stored. When the program is executed by a processor, the steps of the above method are implemented.

[0021] By adopting the embodiment of the present invention, in the absence of GPS positioning information or a survey-grade laser tracker, lidar point cloud data for semantic information acquisition can be constructed quickly and efficiently, and the robot's pose and the final map can be improved.

[0022] With the help of human cognition, batch processing semantic labeling is performed based on the accumulated point cloud map, which improves the labeling efficiency.

[0023] The above description is only an overview of the technical solution of the present invention. In order to be able to understand the technical means of the present invention more clearly, it is implemented in accordance with the content of the description, and in order to make the above and other purposes, features and advantages of the present invention more obvious and understandable, the following specifically describes the embodiments of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] In order to more clearly illustrate the specific embodiments of the present invention or the technical solutions in the prior art, the following will briefly introduce the drawings required for the description of the specific embodiments or the prior art. Obviously, the drawings in the following description are some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0025] Figure 1 is a flowchart of the method for robot point cloud semantic SLAM based on human-in-the-loop optimization according to the embodiment of the present invention;

[0026] Figure 2 is the flowchart of the robot point cloud semantic SLAM method based on human-in-the-loop optimization according to an embodiment of the present invention;

[0027] Figure 3 is the schematic diagram of the robot of the robot point cloud semantic SLAM method based on human-in-the-loop optimization according to an embodiment of the present invention;

[0028] Figure 4 is the mapping result of the underground garage and the corresponding schematic diagram of the ground building of the robot point cloud semantic SLAM method based on human-in-the-loop optimization according to an embodiment of the present invention;

[0029] Figure 5 is the schematic diagram of the error existing in the point cloud map before manual correction of the robot point cloud semantic SLAM method based on human-in-the-loop optimization according to an embodiment of the present invention;

[0030] Figure 6 is the schematic diagram of the point cloud map after manual correction of the robot point cloud semantic SLAM method based on human-in-the-loop optimization according to an embodiment of the present invention;

[0031] Figure 7 is the schematic diagram of the definition of the basement semantic annotation category of the robot point cloud semantic SLAM method based on human-in-the-loop optimization according to an embodiment of the present invention;

[0032] Figure 8 is the schematic diagram of the map semantic annotation interface of the robot point cloud semantic SLAM method based on human-in-the-loop optimization according to an embodiment of the present invention,

[0033] Figure 9 is the schematic diagram of the global point cloud semantics after annotation of the robot point cloud semantic SLAM method based on human-in-the-loop optimization according to an embodiment of the present invention;

[0034] Figure 10 is the schematic diagram of the test of the robot point cloud semantic SLAM method based on human-in-the-loop optimization according to an embodiment of the present invention;

[0035] Figure 11 is the schematic diagram of the front-end and back-end optimization of the robot point cloud semantic SLAM method based on human-in-the-loop optimization according to an embodiment of the present invention;

[0036] Figure 12 is the schematic diagram of the global rough positioning of the Monte Carlo particle filter of the robot point cloud semantic SLAM method based on human-in-the-loop optimization according to an embodiment of the present invention;

[0037] Figure 13 is the schematic diagram of the comparison of the mapping quality of three SLAM algorithms before and after introducing semantic optimization of the robot point cloud semantic SLAM method based on human-in-the-loop optimization according to an embodiment of the present invention;

[0038] Figure 14 It is a schematic diagram of a robot point cloud semantic SLAM system based on human-in-the-loop optimization according to an embodiment of the present invention;

[0039] Figure 15 It is a schematic diagram of a robot point cloud semantic SLAM device based on human-in-the-loop optimization according to an embodiment of the present invention. Specific embodiments

[0040] Next, the technical solutions of the present invention will be clearly and completely described in conjunction with the embodiments. Obviously, the described embodiments are part of the embodiments of the present invention, rather than all of the embodiments. All other embodiments obtained by those of ordinary skill in the art based on the embodiments of the present invention without creative efforts shall fall within the protection scope of the present invention.

[0041] Method embodiments

[0042] According to an embodiment of the present invention, a robot point cloud semantic SLAM method based on human-in-the-loop optimization is provided. Figure 1 It is a flowchart of a robot point cloud semantic SLAM method based on human-in-the-loop optimization according to an embodiment of the present invention, as Figure 1 shown, specifically including:

[0043] A robot point cloud semantic SLAM method based on human-in-the-loop optimization proposed by the present invention specifically includes:

[0044] S1. The lidar on the mobile robot collects data on the target location. During the collection process, based on the SLAM algorithm as the initial mapping algorithm, the relative pose information of each frame of point cloud is obtained, and a map is established;

[0045] S2. Establish a continuous trajectory. After manually observing the mapping error, select the correct point cloud frames around the mapping error for registration optimization of local ICP. At the same time, add the closed-loop edge to the adjacent pose points, construct a dense factor graph, and then perform global optimization based on the factor graph to adjust the overall point cloud error;

[0046] S2 specifically includes: By superimposing the continuous point cloud and the estimated pose corresponding to the continuous point cloud on the map, associating the center points of each frame of point cloud based on the sampling order of the point cloud data to form a continuous trajectory, calculating the closed-loop edge based on the trajectory, and adding the closed-loop edge to construct a new closed loop;

[0047] Obtain the mapping error information observed manually, select the correct point cloud frames around the mapping error for registration optimization of the local registration algorithm based on the mapping error information; obtain the closed loop between the key pose points, construct a dense factor graph, and then perform global optimization based on the factor graph to adjust the overall mapping error, and iterate the registration optimization and global optimization.

[0048] S3. Import the optimized point cloud pose and the corresponding point cloud into the Point Labeler annotation tool, perform semantic annotation of the overlay map based on the predefined point cloud semantic categories, and accumulate the point cloud labeling results to obtain a point cloud semantic segmentation dataset;

[0049] S4. Train and evaluate the segmentation model based on the point cloud semantic segmentation dataset to obtain the trained and evaluated segmentation model;

[0050] S4 specifically includes: dividing the point cloud semantic segmentation dataset into a training set and a test set, training and evaluating the point cloud semantic segmentation model based on deep learning using the training set and the test set, and retaining the model parameters with an overall segmentation accuracy higher than a certain threshold as the model for real-time point cloud semantic segmentation.

[0051] S5. Perform point cloud semantic localization and optimization based on the trained and evaluated segmentation model.

[0052] S5 specifically includes:

[0053] Send the point cloud to the model for real-time point cloud semantic segmentation to obtain the semantic labels of point segmentation, then project a single-frame point cloud n×[x, y, z] onto an x-o-y plane using the lidar Cartesian to polar coordinate projection formula, perform polar coordinate transformation on the xy axes to obtain a 2D feature map, and color the 2D feature map based on the predefined point cloud semantic color matching to obtain a 2D point cloud semantic feature map;

[0054] After obtaining the first frame of point cloud, delete the dynamic semantic points of each frame of point cloud based on the point cloud semantic information, then quickly estimate the 2D pose, use the 2D pose as the initial solution to estimate the 3D point cloud pose, perform 3D point cloud map construction and 2D semantic feature map construction of SLAM, and periodically perform coarse-grained localization of the global 2D semantic feature map on the 2D point cloud feature map of the current scanned point cloud based on Monte Carlo particle filtering;

[0055] Obtain the initial position of the mobile robot on the global point cloud map, correct the odometry front-end estimation, and for the inter-frame point cloud registration during the front-end odometry estimation, use the semantic labels for inter-class separation and intra-class matching to accelerate point cloud search;

[0056] Meanwhile, detect closed loops, and when a new closed loop is found, perform factor graph optimization at the back end to update the global pose information.

[0057] The specific implementation method is as follows:

[0058] Figure 2 It is the workflow diagram of the robot point cloud semantic SLAM method based on human-in-the-loop optimization in the embodiment of the present invention; as Figure 2 shown:

[0059] First, manually control the mobile robot to collect point cloud data in the target scenario. Use lightweight SLAM as the initial mapping algorithm. And use graph optimization as the SLAM backend optimization. The lightweight algorithm can be deployed in the robot's embedded computing unit. However, in cases where the robot's rotation speed is too high and the ground is slippery, the drift error of the odometer is relatively large, and it is prone to degradation in situations with high scene similarity. Therefore, noisy point cloud PC and the corresponding six-degree-of-freedom pose [x, y, z, roll, pitch, y aw sequence are obtained.

[0060] Then, introduce offline human-in-the-loop interactive SLAM optimization. By overlaying the continuous point cloud and the corresponding estimated pose on the map, and associating the center points of each frame of the point cloud based on the sequence to form a continuous trajectory. Manually observe the map to find mapping errors in the point cloud map, generally including ghosting walls, non-smooth ground, and columns. By selecting the point cloud of the pose points near the map error and performing precise registration of local ICP with the neighboring pose point clouds to alleviate the mapping error. And generate loops by manually selecting loops between key pose points or setting corresponding loop conditions (such as: the cumulative distance between the centers of adjacent frames is greater than 10m, but the spatial distance is less than 3m), and perform backend factor graph optimization. The entire process requires continuous manual loop iteration optimization until there are no significant observable mapping errors in the map. The specific optimization time depends on factors such as the size of the scene and the scale of the data collection area.

[0061] Next, based on the optimized point cloud and pose sequence, overlay the map to generate dense point cloud scene information. Manually annotate the point cloud by framing and label the corresponding semantic label information. Overlay annotation improves the annotation efficiency and reduces errors. Use the constructed dataset to train a point cloud segmentation model to extract semantic labels. Disassemble the annotation results back into single-frame point clouds to obtain accurate single-frame point cloud semantic information. Repeat multiple times to construct a dataset for point cloud semantic segmentation, and perform deep learning model training and evaluation for point cloud semantic segmentation to obtain the neural network model weights for real-time segmentation.

[0062] Finally, based on the real-time point cloud and the segmented semantic labels, optimize the semantic SLAM positioning. This algorithm uses the point cloud semantic label information to optimize the front and back ends of the SLAM of the mobile robot respectively.

[0063] Correspondingly, the present invention provides a robot point cloud semantic SLAM method based on human-in-the-loop optimization, including the following steps:

[0064] Figure 3 It is a schematic diagram of the robot of the robot point cloud semantic SLAM method based on human-in-the-loop optimization in an embodiment of the present invention;

[0065] It includes a low-cost lidar, a display, and a computing unit installed on top of the robot, as well as a mobile robot chassis.

[0066] Figure 4 It is the mapping result of the underground garage and the corresponding schematic diagram of the ground building of the robot point cloud semantic SLAM method based on human-in-the-loop optimization in the embodiments of the present invention;

[0067] The target environment for test deployment is as Figure 4 shown, which is a medium and large underground garage in a typical scientific research park. There is no GPS signal and other auxiliary positioning infrastructure in this scenario.

[0068] Step 1: Data collection and annotation of point cloud semantic segmentation based on human-in-the-loop interaction. First, manually control the mobile robot to traverse and move in the target scene to collect data, and enrich the diversity of moving targets in the data by collecting data on different dates multiple times. During this period, any point cloud SLAM algorithm can be used as the initial mapping algorithm to estimate the relative pose information of each frame of point cloud, but there are mapping errors.

[0069] Figure 5 It is a schematic diagram of the errors existing in the point cloud map before manual correction of the robot point cloud semantic SLAM method based on human-in-the-loop optimization in the embodiments of the present invention; it can be seen from the enlarged area in the figure that the constructed map contains a large amount of cumulative offset and errors.

[0070] Step 2: Offline human-machine interactive SLAM pose correction. When the scene range is large and there are similar structures, the robot will generate pose estimation deviations during movement, resulting in distortion of the final point cloud map. For example, for a long corridor, a straight line will be distorted into an arc and degenerate; for artificial buildings, there are phenomena such as overly thick walls and double images of walls. In addition, some large pose offsets or long-term cumulative errors will cause the loop detection to fail. By manually observing the deviation of the point cloud in the map, the above problems are discovered and corrected. Specifically, it includes local ICP optimization and loop optimization. Among them, local ICP optimization observes the pose frames near the area with mapping deviation manually, and sequentially selects the surrounding correct point cloud frames (as the reference) for local ICP registration, and estimates and updates the optimized pose. This step can effectively alleviate the local data that is missed or has poor optimization results during automatic execution. Finally, manually select any adjacent and nearby data frames to add associated edges, or set appropriate loop conditions based on the scene size to add automatic loop associated edges, construct a dense factor graph, and then perform global optimization based on the factor graph to adjust the overall point cloud error. Figure 6 It is a schematic diagram of the point cloud map after manual correction of the robot point cloud semantic SLAM method based on human-in-the-loop optimization in the embodiments of the present invention; the wall surface is clear without double images.

[0071] This step requires continuous manual iterative optimization, and the specific optimization time depends on factors such as the size of the scenario.

[0072] Figure 7 It is a schematic diagram of the definition of the basement semantic annotation category of the robot point cloud semantic SLAM method based on human-in-the-loop optimization in the embodiments of the present invention;

[0073] Define the categories to be annotated for the underground garage scenario, such as Figure 7 shown, including indistinguishable noise points, vehicles, motorcycles, pedestrians, the ground, load-bearing columns, walls, and ceilings.

[0074] Step 3, semantic manual annotation of the scene point cloud. Define the semantic categories of the annotation based on the SLAM map results constructed for the corresponding deployment scenario, including an additional undefined category to handle points that cannot be judged. Import the optimized point cloud pose and the corresponding point cloud into the Point Labeler annotation tool, and perform semantic annotation of the superimposed map based on the predefined point cloud semantic categories. Due to the superimposition of a large number of point clouds, the recognizability of the semantic entities in the scene is greatly improved, which can alleviate the annotation errors caused by the small number of sparse point clouds. At the same time, by annotating in the form of a superimposed map, the annotation efficiency is greatly improved.

[0075] Figure 8 It is a schematic diagram of the map semantic annotation interface of the robot point cloud semantic SLAM method based on human-in-the-loop optimization in the embodiments of the present invention;

[0076] Figure 8 shown, it is a schematic diagram of the underground garage environment where the robot is located and the corresponding point cloud map in the graphical interface of the annotation tool.

[0077] Figure 9 It is a schematic diagram of the global point cloud semantics after annotation of the robot point cloud semantic SLAM method based on human-in-the-loop optimization in the embodiments of the present invention;

[0078] Construct a semantic segmentation data set by accumulating the point cloud annotation results of a certain scale, and the superimposed point cloud semantic map, such as Figure 9 shown:

[0079] Merge the point cloud annotation results of multiple acquisition sequences to construct a point cloud semantic segmentation data set with a scale that meets subsequent model training.

[0080] Step 4: Train the point cloud segmentation model. Divide the dataset labeled in Step 3 into two non - overlapping subsets, one for model training and the other for model evaluation. Based on the training set and the test set, train and evaluate the deep - learning - based point cloud semantic segmentation model, and retain the model parameters with better overall segmentation accuracy as the model for subsequent real - time point cloud semantic segmentation. The training process is carried out on a GPU server with an Intel i7 - 9700 CPU, 16G of memory, a hard disk, and an NVIDIA 2080ti GPU. Set batch = 4, start training using the Adam optimizer with a learning rate of 0.0001, and train all models 80 times.

[0081] Figure 10 It is a test schematic diagram of the human - in - the - loop optimized robot point cloud semantic SLAM method according to the embodiment of the present invention;

[0082] During the test process, three models, RandlaNet, PolarSeg, and Cyclinder3D, were selected to be trained and tested based on the underground garage data. The evaluation results are as Figure 8 shown. Considering both speed and segmentation mIoU accuracy, PolarSeg was selected as the real - time segmentation model for deployment on the robot computing unit.

[0083] Step 5: SLAM algorithm based on semantic optimization. This step has the following processes:

[0084] Process 1: Encapsulate the human - like spatial semantic cognitive ability into the system through point cloud semantic segmentation, and perform semantic segmentation on each frame of the point cloud obtained in real - time. At the same time, based on the labeled map semantics, perform voxel downsampling to obtain the uniform semantic map information of the global map for global positioning. During pre - processing, perform offline normal calculation on the point cloud map and generate a local grid map representation based on semantic labels.

[0085] Figure 11 It is a front - end and back - end optimization schematic diagram of the human - in - the - loop optimized robot point cloud semantic SLAM method according to the embodiment of the present invention;

[0086] Process 2. Cognitive optimization of the existing SLAM system is carried out from two aspects: filtering of unstable point clouds at the front end and optimization of closed-loop registration at the back end. When calculating the inter-frame registration of the front-end odometer estimation, dynamic and potentially dynamic (stopped vehicles) are removed, and the semantic information of static targets is preferentially considered for registration; and the search efficiency between features is improved through intra-class constraints. During the back-end closed-loop detection, for a frame of semantic point cloud at time t, a nearest-neighbor search is performed to construct a polar coordinate encoding of the 2D environmental frame based on semantics (Semantic Scan Context, SSC) for rough pose detection to obtain the accurate information of [x; y; yaw] of the [R; t] matrix, which is used as an initial value to accelerate the accurate registration of ICP. On the one hand, the registration accuracy of a single frame is improved, and at the same time, the success rate of closed-loop detection is increased.

[0087] Figure 12 It is a schematic diagram of global rough positioning of the Monte Carlo particle filter of the robot point cloud semantic SLAM method based on human-in-the-loop optimization in the embodiment of the present invention;

[0088] Process 3. Relocalization based on particle filter. Global positioning correction is performed during initial startup and relocalization at regular intervals. Global uniform seed points are scattered through the global grid map, and rough positioning estimation is performed based on the particle filter of Monte Carlo localization. Then, based on the similarity between the semantic polar coordinate encoding of the current scan frame point cloud and the semantic polar coordinate encoding of the map, the current position of the robot is inferred. It is provided to the front-end odometer in step 2 as an initial solution for subsequent calculations.

[0089] Based on the point cloud, polar coordinate projection is performed to generate a polar coordinate semantic environment encoding for rough pose estimation between frames, and then accurate pose estimation of ICP or NDT is performed based on the initial solution. Then, the closed loop is detected. When a new closed loop is found, factor graph optimization at the back end is performed to update the global pose information;

[0090] At regular intervals, global positioning of the robot based on Monte Carlo particle filter is performed to periodically correct the cumulative error;

[0091] For Process 1, the human semantic cognitive ability for point clouds is encapsulated into the map and each frame of point cloud. The operation is as follows. Each time the robot obtains a new scan, first the point cloud is input into the deep learning segmentation model to obtain the label of the point direction. Then, a single-frame point cloud n×[x, y, z] is projected onto a two-dimensional map [W, H] using the lidar Cartesian-to-polar projection formula. The projection formula is as follows:

[0092]

[0093] where, is the distance of the point, F v = Fov up + Fovdown = 30° is the vertical angle, H scan = 2° is the vertical resolution, H r = Fov v / H scan +1 = 16, W r = 18000 is the horizontal angular resolution of the lidar. F up = Fov up ; r is the radius, xyz are the Cartesian coordinates of the point cloud, F = Fov, Fov is the abbreviation of the field of view angle, H scan is the vertical angular resolution; F v = Fov up + Fov down = 30° means that the sum of the upper and lower vertical sub - fields of view angles is equal to 30°.

[0094] H r = 30 / 2 + 1 = 16 is the number of radar vertical beams;

[0095] w r = 18000 is the number of scan sampling points in the case of a horizontal angular resolution of 0.02 degrees;

[0096] For process 2, every time the robot is initially started or needs to be re - located at regular intervals, the particles are evenly distributed on the semantic map through Monte Carlo localization (MCL) based on a particle filter. Once the particles are scattered anywhere on the map, a semantic range map can be generated from the corresponding particles.

[0097] MCL implements a recursive Bayesian filter for estimating the probability density Based on the observation data z from the initial time to time t 1：t and the motion estimation Estimate the pose x at time t t . The motion state update function is:

[0098]

[0099] Among them, η represents the normalization constant, represents the motion model, represents the observation model, while represents the prior probability model of the previous - moment velocity.

[0100] Using semantic label indexing helps to reduce the variability of the range map distribution. To reduce complexity, the motion of the robot is restricted from 6 degrees of freedom [X, Y, Z, Roll, Pitch, Y aw to 3 degrees of freedom [X, Y, Y aw, only consider the motion in a two-dimensional plane. Therefore, the positioning of the robot is L r =(x, y, y aw ), L r The corresponding observation is the semantic range map SRM r =(W, H).

[0101] For process 3), compare the current range map generated by scanning the SRM r =(W, H) with the range maps SRM p(i＝0，...，n) =(W, H) of all particles, and calculate their similarity. Then infer the current position of the robot from the semantic map with the highest similarity. The observation model can be defined as the average of the absolute pixel-level differences between two images of the same scale divided by (W*H).

[0102]

[0103] Among them, SRM r represents the generated current range map, SRM p represents the range maps of all particles, and (W, H) represents the observed semantic map.

[0104] Figure 13 is a schematic diagram of the comparison of the mapping quality of three SLAM algorithms in the semantic SLAM method of the robot point cloud based on human-in-the-loop optimization before and after the introduction of semantic optimization in the embodiments of the present invention;

[0105] Compares the mapping effects of three pure point cloud SLAM algorithms, LOAM, LeGO-LOAM and LIO-SAM, and the mapping effects after adding point cloud semantic optimization. Among them, MME, MPV and MOM are all quantitative evaluation indicators based on the map topological information entropy. At three different voxelization densities of 0.2, 0.4 and 0.5, by increasing semantic optimization, the evaluation indicators of the corresponding algorithms all decrease relatively. It shows that the semantic optimization strategy can effectively reduce the topological information entropy of the overall map, making the map more stable, and indirectly indicating that the corresponding continuous pose estimation accuracy is better. Otherwise, a large number of ghost walls will be generated, resulting in a higher result of the map point cloud topological entropy.

[0106] The advantages of the present invention are as follows:

[0107] A semantic SLAM method for robot point cloud based on human-in-the-loop optimization is proposed. This method can improve the positioning of mobile robots in indoor environments by only using low-cost lidar.

[0108] In the absence of GPS positioning information or survey-grade laser trackers, quickly and efficiently construct lidar point cloud data for semantic information collection, and improve the robot's pose and the final map.

[0109] With the help of human cognition, batch semantic annotation is performed based on the accumulated point cloud map. This greatly improves the labeling efficiency.

[0110] Introduce human-machine interaction collaboration into the entire semantic SLAM workflow: interactive SLAM and semantic data labeling to obtain a point cloud semantic map that is consistent with the actual height.

[0111] Utilize the point cloud semantic segmentation model to simulate the cognitive ability of humans to obtain real-time point cloud semantic information, improving speed and accuracy.

[0112] Propose a method for optimizing the front and rear ends of SLAM based on point cloud semantics, and combine semantic-based Monte Carlo particle filtering for periodic global positioning to reduce cumulative errors and map distortion.

[0113] System embodiment

[0114] According to an embodiment of the present invention, a robot point cloud semantic SLAM system based on human-in-the-loop optimization is provided. Figure 14 It is a schematic diagram of the robot point cloud semantic SLAM system based on human-in-the-loop optimization according to an embodiment of the present invention, as Figure 14 shown, specifically including:

[0115] An acquisition module, installed on a mobile robot, for collecting data on a target location. During the collection process, the SLAM algorithm is used as the initial mapping algorithm to calculate the relative pose information of each frame of point cloud and complete mapping.

[0116] An optimization module, for establishing a continuous trajectory, obtaining the mapping error information observed manually, based on the mapping error information, selecting the correct point cloud frames around the mapping error to re-perform the registration optimization of local ICP, obtaining the closed loop between key pose points, constructing a dense factor graph, and then performing global optimization based on the dense factor graph to adjust the overall mapping error, iteratively performing registration optimization and global optimization.

[0117] A dataset module, for importing the point cloud pose after local ICP registration optimization and the corresponding point cloud of the optimized point cloud pose into a labeling tool, and based on predefined point cloud semantic categories, using the labeling tool to perform point cloud semantic annotation of the superimposed map, and accumulating the point cloud semantic marking results to obtain a point cloud semantic segmentation dataset.

[0118] A segmentation model module, for training and evaluating a segmentation model based on the point cloud semantic segmentation dataset to obtain a trained and evaluated segmentation model.

[0119] A positioning optimization module, for performing point cloud semantic positioning and optimization based on the trained and evaluated segmentation model.

[0120] The optimization module is specifically configured to: perform map overlay on the continuous point cloud and the estimated pose corresponding to the continuous point cloud, associate the center points of each frame of point cloud based on the sampling order of the point cloud data to form a continuous trajectory, calculate the closed-loop edge based on the trajectory, and add the closed-loop edge to construct a new closed loop;

[0121] Obtain the mapping error information observed manually, select the correct point cloud frames around the mapping error based on the mapping error information for registration optimization of the local registration algorithm; obtain the closed loop between key pose points, construct a dense factor graph, and then perform global optimization based on the factor graph to adjust the overall mapping error, and iterate the registration optimization and global optimization.

[0122] The segmentation model module is specifically configured to: divide the point cloud semantic segmentation dataset into a training set and a test set, train and evaluate the point cloud semantic segmentation model based on deep learning using the training set and the test set, and retain the model parameters with an overall segmentation accuracy higher than a certain threshold as the model for real-time point cloud semantic segmentation.

[0123] The positioning optimization module is specifically configured to:

[0124] Send the point cloud to the model for real-time point cloud semantic segmentation to obtain the semantic labels of point segmentation, then project a single-frame point cloud n×[x, y, z] onto an x-o-y plane using the lidar Cartesian to polar projection formula, perform polar coordinate transformation on the xy axes to obtain a 2D feature map, and color the 2D feature map based on the predefined point cloud semantic color matching to obtain a 2D point cloud semantic feature map;

[0125] After obtaining the first frame of point cloud, delete the dynamic semantic points of each frame of point cloud based on the point cloud semantic information, then quickly estimate the 2D pose, use the 2D pose as the initial solution to estimate the 3D point cloud pose, perform 3D point cloud map construction and 2D semantic feature map construction of SLAM, and periodically perform coarse-grained positioning of the global 2D semantic feature map on the 2D point cloud feature map of the current scanned point cloud based on Monte Carlo particle filtering;

[0126] Obtain the initial position of the mobile robot on the global point cloud map, correct the odometer front-end estimation, and for the inter-frame point cloud registration during the front-end odometer estimation, use the semantic labels for inter-class separation and intra-class matching to accelerate the point cloud search;

[0127] At the same time, detect the closed loop, and when a new closed loop is found, perform factor graph optimization at the back end to update the global pose information.

[0128] Device Embodiment 1

[0129] An embodiment of the present invention provides a robot point cloud semantic SLAM device based on human-in-the-loop optimization, as Figure 15As shown, it includes: a memory 150, a processor 152, and a computer program stored on the memory 150 and executable on the processor 152. When the computer program is executed by the processor, the steps in the above method embodiments are implemented.

[0130] Device Embodiment 2

[0131] An embodiment of the present invention provides a computer-readable storage medium. An implementation program for information transmission is stored on the computer-readable storage medium. When the program is executed by the processor 152, the steps in the above method embodiments are implemented.

[0132] Finally, it should be noted that: the above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: they can still modify the technical solutions described in the foregoing embodiments, or perform equivalent replacements on some or all of the technical features; and these modifications or replacements do not make the essence of the technical solutions of the various embodiments of the present invention deviate from the scope of this solution.

Claims

1. A robot point cloud semantic SLAM method optimized based on human-in-the-loop, characterized in that, including S1. Use the acquisition module on the mobile robot to collect data from the target location. During the collection process, use the SLAM algorithm as the initial mapping algorithm to calculate the relative pose information of each frame of point cloud, and complete the mapping; S2. Establish a continuous trajectory of the point cloud, obtain the mapping error information observed manually, select the correct point cloud frames around the mapping error based on the mapping error information, re - perform the registration optimization of local ICP, obtain the closed - loop between key pose points, construct a dense factor graph, and then perform global optimization based on the dense factor graph to adjust the overall mapping error, and iterate the registration optimization and global optimization; specifically including: By overlaying the continuous point cloud and the estimated pose corresponding to the continuous point cloud on the map, associate the center points of each frame of point cloud based on the sampling order of the point cloud data to form a continuous trajectory, calculate the closed - loop edge based on the trajectory, and add the closed - loop edge to construct a new closed - loop; Obtain the mapping error information observed manually, select the correct point cloud frames around the mapping error to perform the registration optimization of the local registration algorithm; obtain the closed - loop between key pose points, construct a dense factor graph, and then perform global optimization based on the factor graph to adjust the overall mapping error, and iterate the registration optimization and global optimization; S3. Import the point cloud pose after local ICP registration optimization and the corresponding point cloud of the optimized point cloud pose into the annotation tool. Based on the predefined point cloud semantic categories, use the annotation tool to perform the semantic annotation of the point cloud for the overlaid map, and accumulate the point cloud semantic labeling results to obtain the point cloud semantic segmentation dataset; S4. Train and evaluate the point cloud semantic segmentation model based on the point cloud semantic segmentation dataset, specifically including: Divide the point cloud semantic segmentation dataset into a training set and a test set, train and evaluate the deep - learning - based point cloud semantic segmentation model based on the training set and the test set, and retain the model parameters with an overall segmentation accuracy higher than a certain threshold as the model for real - time point cloud semantic segmentation; S5. Based on the trained and evaluated segmentation model, perform point cloud semantic localization and optimization, specifically including: Send the point cloud to the model for real - time point cloud semantic segmentation to obtain the semantic label of the point segmentation, then project a single - frame point cloud \(n\times[x,y,z]\) onto an \(x - o - y\) plane using the lidar Cartesian - to - polar projection formula, perform polar coordinate transformation on the \(x\) and \(y\) axes to obtain a 2D feature map, and color the 2D feature map based on the predefined point cloud semantic color matching to obtain a 2D point cloud semantic feature map; After obtaining the first frame of point cloud, delete the dynamic semantic points of each frame of point cloud based on the point cloud semantic information, then quickly estimate the 2D pose, use the 2D pose as the initial solution to estimate the 3D point cloud pose, perform the construction of the 3D point cloud map and the 2D semantic feature map of SLAM, and periodically perform the coarse - grained localization of the global 2D semantic feature map for the 2D point cloud feature map of the current scanned point cloud based on the Monte Carlo particle filter; Obtain the initial position of the mobile robot on the global point cloud map, correct the odometer front-end estimation. For the inter-frame point cloud registration during the front-end odometer estimation, use semantic tags for inter-class separation and intra-class matching to accelerate the point cloud search; Meanwhile, detect closed loops. When a new closed loop is found, perform factor graph optimization at the back end to update the global pose information.

2. A robot point cloud semantic SLAM system optimized based on human-in-the-loop, characterized in that, Including, The acquisition module, installed on the mobile robot, is used to collect data from the target location. During the collection process, the SLAM algorithm is used as the initial mapping algorithm to calculate the relative pose information of each frame of point cloud and complete the mapping; The optimization module is used to establish a continuous trajectory, obtain the mapping error information observed manually, re-perform the registration optimization of local ICP based on the correct point cloud frames around the mapping error based on the mapping error information, obtain the closed loop between key pose points, construct a dense factor graph, and then perform global optimization based on the dense factor graph to adjust the overall mapping error, and iterate the registration optimization and global optimization; Specifically used for: By superimposing the continuous point cloud and the estimated pose corresponding to the continuous point cloud on the map, associate the center points of each frame of point cloud based on the sampling order of the point cloud data to form a continuous trajectory, calculate the closed loop edge based on the trajectory, and add the closed loop edge to construct a new closed loop; Obtain the mapping error information observed manually, and re-perform the registration optimization of the local registration algorithm based on the correct point cloud frames around the mapping error based on the mapping error information; Obtain the closed loop between key pose points, construct a dense factor graph, and then perform global optimization based on the factor graph to adjust the overall mapping error, and iterate the registration optimization and global optimization; The dataset module is used to import the point cloud pose after local ICP registration optimization and the point cloud corresponding to the optimized point cloud pose into the annotation tool, and perform semantic annotation of the point cloud on the superimposed map using the annotation tool based on the predefined point cloud semantic categories, and accumulate the point cloud semantic labeling results to obtain the point cloud semantic segmentation dataset; The segmentation model module is used to train and evaluate the segmentation model based on the point cloud semantic segmentation dataset; Specifically used for: Divide the point cloud semantic segmentation dataset into a training set and a test set, train and evaluate the point cloud semantic segmentation model based on deep learning using the training set and the test set, and retain the model parameters with an overall segmentation accuracy higher than a certain threshold as the model for real-time point cloud semantic segmentation; The positioning optimization module is used to perform point cloud semantic positioning and optimization based on the trained and evaluated segmentation model; Specifically used for: Send the point cloud to the model for real-time point cloud semantic segmentation to obtain the semantic label of the point segmentation, then project a single frame of point cloud n×[x, y, z] onto an x-o-y plane using the lidar Cartesian to polar coordinate projection formula, perform polar coordinate transformation on the xy axis to obtain a 2D feature map, and color the 2D feature map based on the predefined point cloud semantic color scheme to obtain a 2D point cloud semantic feature map; After obtaining the first-frame point cloud, dynamic semantic points of each frame of point cloud are deleted based on the point cloud semantic information, and then the 2D pose is quickly estimated. Using the 2D pose as the initial solution, the 3D point cloud pose is estimated, and the 3D point cloud map construction and 2D semantic feature map construction of SLAM are carried out. Periodically, based on Monte Carlo particle filtering, coarse-grained localization of the global 2D semantic feature map is performed on the 2D point cloud feature map of the current scanned point cloud. The initial position of the mobile robot on the global point cloud map is obtained, and the front-end odometry estimation is corrected. For the inter-frame point cloud registration during the front-end odometry estimation, semantic labels are used for inter-class separation and intra-class matching to accelerate the point cloud search. At the same time, loop closures are detected. When a new loop closure is found, factor graph optimization of the backend is performed to update the global pose information.

3. A robot point cloud semantic SLAM device based on human-in-the-loop optimization, characterized in that, It includes: A memory, a processor, and a computer program stored on the memory and executable on the processor. When the computer program is executed by the processor, the steps of the robot point cloud semantic SLAM method based on human-in-the-loop optimization as claimed in claim 1 are implemented.

4. A computer-readable storage medium, characterized in that, An implementation program for information transmission is stored on the computer-readable storage medium. When the program is executed by the processor, the steps of the robot point cloud semantic SLAM method based on human-in-the-loop optimization as claimed in claim 1 are implemented.

Images