A semantic mapping and localization system for mobile robots based on 3D LiDAR

Abstract

This invention discloses a semantic mapping and localization system for mobile robots based on 3D LiDAR, specifically relating to the field of artificial intelligence. The system includes a computer, a radar response module, a data acquisition system, a data storage module, a data processing system, and a mobile device. By introducing and integrating semantic information, the system not only provides geographic location data but also understands the semantic structure of the environment, thereby achieving more stable and accurate navigation and localization in dynamic environments. Through deep learning models, semantic segmentation of the LiDAR point cloud is performed, extracting key semantic information from the environment in real time, providing a richer environmental description for mapping and localization. This system can more accurately identify and locate key features in the environment, such as road boundaries, buildings, and other important landmarks. This precise environmental understanding makes the localization results more stable, especially in scenes with many dynamic objects.

Description

Technical Field

[0001] This invention relates to the field of artificial intelligence technology, and in particular to a semantic mapping and localization system for mobile robots based on 3D LiDAR. Background Technology

[0002] Currently, in the fields of mobile robotics and autonomous driving, navigation, localization, and environmental modeling primarily rely on LiDAR (Light Detection and Ranging) technology. Existing LiDAR SLAM (Simultaneous Localization and Mapping) systems, such as LOAM (Lidar Odometry and Mapping), achieve localization and map building by collecting laser point cloud data from the environment. However, these systems face challenges in accuracy and robustness when dealing with complex or dynamic environments.

[0003] While existing LiDAR SLAM systems can provide basic mapping and localization functions, they often cannot effectively identify and handle dynamic objects in the environment, such as passing vehicles and pedestrians, which leads to a decrease in map accuracy and system stability. Furthermore, traditional LiDAR SLAM systems lack the utilization of environmental semantic information, limiting their application in complex scenarios. Therefore, this invention proposes a semantic mapping and localization system for mobile robots based on 3D LiDAR. Summary of the Invention

[0004] The purpose of this invention is to overcome the shortcomings of existing technologies and propose a semantic mapping and localization system for mobile robots based on 3D LiDAR.

[0005] To achieve the above objectives, the present invention adopts the following technical solution: a semantic mapping and localization system for mobile robots based on 3D LiDAR, comprising a computer, a radar response module, a data acquisition system, a data storage module, a data processing system, and a mobile device;

[0006] The data acquisition system includes a laser emitter for emitting laser light, a scanning mechanism unit, a photodetector, a data processing unit, and a data uploading unit;

[0007] The data processing system includes a data receiving unit, a data interaction unit, a data analysis unit, an algorithm learning unit, and a data processing unit for receiving data.

[0008] The mobile device includes a trajectory simulation module, a signal transmission module, a high-speed camera module, an infrared camera module, a motion control module, and a radar module;

[0009] Specifically, the steps of the semantic mapping and localization system for mobile robots based on 3D LiDAR are as follows:

[0010] S1. The original point cloud of the lidar is used as input, and the semantic information of the lidar point cloud is obtained by the lidar point cloud semantic segmentation network.

[0011] In the S2 laser semantic mapping module, planar points and edge points of the laser semantic point cloud are acquired. These feature points are combined with adaptive semantic weight factors to calculate the laser odometry and perform graph optimization to generate a point cloud map.

[0012] S3. Combine closed-loop thread to perform closed-loop detection and perform global map optimization to finally generate semantic NDT graph;

[0013] S4. The input LiDAR semantic point cloud is combined with the prior global semantic NDT graph for global localization. After semantic verification, the initial pose is obtained. The subsequent frame-to-graph matching is performed based on the initial pose. Combined with adaptive semantic information constraints, the current LiDAR pose is obtained through coarse matching and fine matching.

[0014] Preferably, in step S2, the planar point features and edge point features in the point cloud are used for registration to obtain the transformation relationship between frames, and then a point cloud map is constructed. The specific construction steps are as follows:

[0015] pi is the smoothness value of the point to be calculated, and pj are the n nearest points to pi within the same ring. The formula is as follows:

[0016]

[0017] Based on the smoothness level, edge points and planar points in the point cloud are distinguished according to a threshold and semantic information verification. To ensure uniform distribution of feature points, each ring is divided into Nr sub-regions. Within each region, points are sorted and selected. Finally, Ne points with the lowest smoothness are selected as edge features, and Np points with the highest smoothness are selected as planar features. The final result is the edge feature cloud Ce and the planar feature cloud Cp of the point cloud for that frame, as shown in the following formula:

[0018] d ε The distance is the distance from the i-th edge point at time k+1 to the straight line formed by the two edge points j and l at time k.

[0019]

[0020] The distance is the distance from the i-th plane point at time k+1 to the plane formed by the three points j, l, and m at time k.

[0021]

[0022] As shown in the formula below, F is the overall optimization objective function, dε and These correspond to the distance from the target point to the edge feature and the distance from the target point to the planar feature, respectively, where w is the semantic weight factor for each target point.

[0023]

[0024] The Jacobian of the objective function F is calculated, and the formula is divided into two parts by the chain rule:

[0025]

[0026] The first part of the chain rule: where wi is the semantic weight factor, a ε b ε c ε This is the vector perpendicular to the target line from the edge feature. The normal vector of the target plane:

[0027]

[0028]

[0029] The second part of the chain rule involves differentiating the translations in the transformation matrix:

[0030] Differentiate the rotations in the transformation matrix:

[0031]

[0032] Preferably, in step S4, lightweight saving and reading are performed using a semantic NDT map. When the system performs real-time positioning, the information of the local point cloud is recovered by the distribution of semantic NDT cells. The strategy for generating a semantic NDT map is as follows: semantic NDT cells are dynamically generated based on the semantic information of the point cloud.

[0033]

[0034] Preferably, in step S4, the localization module uses a normal distribution from distribution to distribution to solve the problem. First, the space is discretized into a voxel distribution, where Sc is the number of point clouds in a NDT voxel, and p is the set of point clouds in that voxel. The expression of each NDT voxel is as follows:

[0035]

[0036] For a 3D point cloud, the probability density expression for each NDT voxel unit is:

[0037]

[0038] The formula for calculating the distance between voxels c and l is:

[0039]

[0040] Where ucl is the vector distance from uc to ul after transformation:

[0041] μ cl (p)=Rμ c +t-μ l ;

[0042] The location is determined using a scan-to-map matching method. This is the current radar frame point cloud set. For the submap point cloud set, It is a sub-map generated based on the radar's current world coordinates and effective scanning range. The sub-map is obtained by resampling the semantic NDT map. c,l Let the semantic information weighting factor be the objective function, and the expression for the objective function be:

[0043]

[0044] The gradient vector of the objective function f(T) can be obtained by taking the partial derivative at point p, and the derivative is expressed as follows:

[0045]

[0046] Preferably, in the semantic mapping and localization system, by calculating the number of point clouds belonging to the same category as the current point and the total number of feature point clouds, the weight reflects the proportion of point clouds belonging to the same category, thereby reducing the mismatch of points from different categories. The formula for this semantic weight is:

[0047]

[0048] Preferably, the distance residual is included in the semantic weight calculation, and the specific formula is as follows:

[0049]

[0050] For surface features, semantic weight calculation follows the same principle, and the distance residual is shown in the formula:

[0051]

[0052] Preferably, during the semantic mapping process, the final formula for the adaptive semantic weights of edge features and surface features is:

[0053]

[0054] The present invention is further configured as follows: During the difference calibration process, the images captured by the unmanned equipment are first compared frame by frame with the sample images. The sample images and captured images are matched with time nodes as the matching standard. The illuminance of the captured images is compared with the illuminance of the corresponding time node in the sample images. If it is too strong, the light intensity of the image is automatically reduced; if it is too weak, the light intensity of the image is automatically increased. At the same time, when there is a difference between the wind force and the sample image data, the position of the unmanned equipment displayed by the radar positioning is compared with the position of the unmanned equipment in the sample data. Then, the wind force is compared with the wind force in the sample data. The wind force data is balanced with the conformity of the radar positioning data as the standard. Finally, the sample images and captured images are retrieved within the same time coordinate for image difference comparison. If the similarity exceeds 95%, there is no difference.

[0055] By adopting the above technical solutions, the negative impact of environmental factors on unmanned equipment photography can be reduced.

[0056] The beneficial effects of this invention are as follows:

[0057] This invention introduces and integrates semantic information, enabling the system to not only provide geographic location data but also understand the semantic structure of the environment. This allows for more stable and accurate navigation and positioning in dynamic environments. By using a deep learning model to perform semantic segmentation on laser point clouds and extracting key semantic information from the environment in real time, the system provides a richer environmental description for mapping and positioning. This system can more accurately identify and locate key features in the environment, such as road boundaries, buildings, and other important landmarks. This precise environmental understanding makes positioning more stable, especially in dynamic or visually complex scenes.

[0058] By leveraging the latest deep learning technology to perform real-time semantic analysis on LiDAR data, this system can more accurately identify and classify various objects in complex environments compared to traditional methods. For users of autonomous vehicles and service robots, this provides a more reliable and consistent navigation experience. Robots can interact with their environment more naturally, such as avoiding obstacles and identifying designated areas, significantly improving user satisfaction and system usability. Attached Figure Description

[0059] Figure 1 This invention presents a system diagram of a semantic mapping and localization system for mobile robots based on 3D LiDAR;

[0060] Figure 2 The present invention presents a flowchart of a semantic mapping and localization system for mobile robots based on 3D LiDAR. Detailed Implementation

[0061] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0062] Example

[0063] like Figure 1-2 As shown, the present invention provides a technical solution: a semantic mapping and localization system for mobile robots based on 3D LiDAR, including a computer, a radar response module, a data acquisition system, a data storage module, a data processing system, and a mobile device;

[0064] In this embodiment, the mobile device is not limited to intelligent robots, unmanned vehicles, or other intelligent mobile devices.

[0065] The data acquisition system includes a laser emitter for emitting laser light, a scanning mechanism unit, a photodetector, a data processing unit, and a data uploading unit;

[0066] The data processing system includes a data receiving unit, a data interaction unit, a data analysis unit, an algorithm learning unit, and a data processing unit for receiving data.

[0067] The mobile device includes a flight path recording module, a signal transmission module, a high-speed camera module, an infrared camera module, a driving control module, and a radar module;

[0068] Specifically, the steps of the semantic mapping and localization system for mobile robots based on 3D LiDAR are as follows:

[0069] S1. The original point cloud from the LiDAR is used as input, and its semantic information is obtained through a LiDAR point cloud segmentation network. Specifically:

[0070] Input: Raw point cloud data from 3D LiDAR.

[0071] Processing: Semantic segmentation of point clouds is performed using pre-trained deep neural networks. The network architecture can be based on 3D convolution or point cloud-specific network models (such as PointNet or SparseConvNet) to distinguish different environmental elements.

[0072] Output: Point cloud with semantic labels, where each point is labeled with its corresponding category (e.g., road, building, vehicle, etc.);

[0073] In the S2 laser semantic mapping module, planar points and edge points of the laser semantic point cloud are acquired. Laser odometry is calculated using these points, and graph optimization is performed to generate a point cloud map. Combined with real-time extracted semantic information, the odometry calculation and map construction of the LiDAR are optimized. During the feature extraction stage, the weights of feature points are adjusted based on semantic labels (e.g., road edges are more important than tree leaves).

[0074] S3. Combine closed-loop thread for closed-loop detection, use semantic labels to improve the closed-loop detection algorithm, match known semantic features in the map (such as building corners) to more accurately identify and correct accumulated errors, and perform global map optimization to finally generate a semantic NDT map.

[0075] S4. The input LiDAR semantic point cloud is combined with the prior global semantic NDT graph for global localization. After semantic verification, the initial pose is obtained. The subsequent frame-to-graph matching is performed based on the result of the initial pose. Combined with semantic information constraints, the current LiDAR pose is obtained through coarse matching and fine matching. The improved Normal Distribution Transform (NDT) method is used to match the point cloud in combination with semantic labels.

[0076] In this embodiment, a deep learning model is used to perform semantic segmentation on the LiDAR point cloud, extracting key semantic information from the environment in real time. This provides a richer environmental description for mapping and positioning. The LOAM method, improved by semantic information, enhances the global consistency of the map. Especially in complex or changing environmental conditions, the latest deep learning technology is used to perform real-time semantic parsing on LiDAR data. Compared with traditional methods, it can more accurately identify and classify various objects in complex environments. By integrating semantic information, the system can not only provide geographic location data but also understand the semantic structure of the environment, thereby achieving more stable and accurate navigation and positioning in dynamic environments.

[0077] Working principle:

[0078] This system is divided into two parts: the first part is the laser semantic mapping process, and the second part is the localization process based on the laser semantic map. Figure 2 As shown, the original point cloud of the LiDAR is used as input. After passing through a LiDAR point cloud segmentation network, semantic information of the LiDAR point cloud is obtained. In the LiDAR semantic mapping module, planar points and edge points of the LiDAR semantic point cloud are obtained. LiDAR odometry is calculated using these planar points and edge points, and graph optimization is performed. Simultaneously, a loop closure thread is used for loop closure detection, and global map optimization is performed to finally generate a semantic NDT graph. In the localization module, the input LiDAR semantic point cloud is combined with the prior global semantic NDT graph for global localization. After semantic verification, an initial pose is obtained. Based on the initial pose, subsequent frame-to-graph matching is performed. Combined with semantic information constraints, coarse matching and fine matching are used to obtain the current LiDAR pose.

[0079] The transformation relationship between frames is obtained by registering planar point features and edge point features in the point cloud, and then a point cloud map is constructed. The specific construction steps are as follows:

[0080] pi is the smoothness value of the point to be calculated, and pj are the n nearest points to pi within the same ring. The formula is as follows:

[0081]

[0082] Based on the smoothness level, edge points and planar points in the point cloud are distinguished according to a threshold and semantic information verification. To ensure uniform distribution of feature points, each ring is divided into Nr sub-regions. Within each region, points are sorted and selected. Finally, Ne points with the lowest smoothness are selected as edge features, and Np points with the highest smoothness are selected as planar features. The final result is the edge feature cloud Ce and the planar feature cloud Cp of the point cloud for that frame, as shown in the following formula:

[0083] d ε The distance is the distance from the i-th edge point at time k+1 to the straight line formed by the two edge points j and l at time k.

[0084]

[0085] The distance is the distance from the i-th plane point at time k+1 to the plane formed by the three points j, l, and m at time k.

[0086]

[0087] As shown in the formula below, F is the overall optimization objective function, d ε and These correspond to the distance from the target point to the edge feature and the distance from the target point to the planar feature, respectively, where w is the semantic weight factor for each target point.

[0088]

[0089] The Jacobian of the objective function F is calculated, and the formula is divided into two parts by the chain rule:

[0090]

[0091] The first part of the chain rule: where wi is the semantic weight factor, a ε b ε c ε This is the vector perpendicular to the target line from the edge feature. The normal vector of the target plane:

[0092]

[0093] The second part of the chain rule involves differentiating the translations in the transformation matrix:

[0094] Differentiate the rotations in the transformation matrix:

[0095]

[0096] Lightweight saving and reading are achieved using semantic NDT maps. During real-time positioning, the distribution of semantic NDT cells is used to recover local point cloud information. The strategy for generating semantic NDT maps is as follows: semantic NDT cells are dynamically generated based on the semantic information of the point cloud.

[0097]

[0098] The localization module uses a normal distribution from distribution to distribution to solve the problem. First, the space is discretized into a voxel distribution, where Sc is the number of point clouds in a single NDT voxel, and p is the set of point clouds in that voxel. Each NDT voxel is represented as follows:

[0099]

[0100]

[0101] For a 3D point cloud, the probability density expression for each NDT voxel unit is:

[0102]

[0103] The formula for calculating the distance between voxels c and l is:

[0104]

[0105] Where ucl is the vector distance from uc to ul after transformation:

[0106] μ cl (p)=Rμ c +t-μ l ;

[0107] The location is determined using a scan-to-map matching method. This is the current radar frame point cloud set. For the submap point cloud set, It is a sub-map generated based on the radar's current world coordinates and effective scanning range. The sub-map is obtained by resampling the semantic NDT map. c，l Let the semantic information weighting factor be the objective function, and the expression for the objective function be:

[0108]

[0109] The gradient vector of the objective function f(T) can be obtained by taking the partial derivative at point p, and the derivative is expressed as follows:

[0110]

[0111] In semantic mapping and localization systems, by calculating the number of points belonging to the same category as the current point and the total number of feature point clouds, the weight reflects the proportion of points from the same category, thereby reducing the mismatch between points from different categories. The formula for this semantic weight is:

[0112]

[0113] Preferably, the distance residual is included in the semantic weight calculation, and the specific formula is as follows:

[0114]

[0115] For surface features, semantic weight calculation follows the same principle, and the distance residual is shown in the formula:

[0116]

[0117] In the process of semantic mapping, the final formula for the adaptive semantic weights of edge features and surface features is:

[0118]

[0119] In the above embodiments, 3D point clouds with semantic information are used as input to the system. In the mapping module, a point cloud map is generated after SemanticLOAM, followed by the generation and saving of a semantic NDT map. In the positioning module, the 3D semantic point cloud is received in real time, and the saved semantic NDT map is resampled to generate a local semantic point cloud map. The two parts of the point cloud are then registered using semantic NDT to obtain real-time positioning information. Compared with traditional solutions, this invention introduces semantic information, resulting in more ideal positioning results in highly dynamic environments and unstructured scenarios. During the mapping process, dynamic vehicles and pedestrians are removed based on the semantic information of the point cloud, while stable semantic feature point clouds are retained to improve positioning stability. Repetitive semantic feature point clouds are downsampled to save data storage space.

[0120] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any other way. Any person skilled in the art may make changes or modifications to the above-disclosed technical content to create equivalent embodiments for application in other fields. However, any simple modifications, equivalent changes, and modifications made to the above embodiments based on the technical essence of the present invention without departing from the scope of the present invention shall still fall within the protection scope of the present invention.

Claims

1. A semantic mapping and localization system for mobile robots based on 3D LiDAR, characterized in that: This includes computers, radar response modules, data acquisition systems, data storage modules, data processing systems, and mobile devices; The data acquisition system includes a laser emitter for emitting laser light, a scanning mechanism unit, a photodetector, a data processing unit, and a data uploading unit; The data processing system includes a data receiving unit, a data interaction unit, a data analysis unit, an algorithm learning unit, and a data processing unit for receiving data. The mobile device includes a trajectory simulation module, a signal transmission module, a high-speed camera module, an infrared camera module, a motion control module, and a radar module; Specifically, the steps of the semantic mapping and localization system for mobile robots based on 3D LiDAR are as follows: S1. The original point cloud of the lidar is used as input, and the semantic information of the lidar point cloud is obtained by the lidar point cloud semantic segmentation network. In the S2 laser semantic mapping module, planar points and edge points of the laser semantic point cloud are acquired. These feature points are combined with adaptive semantic weight factors to calculate the laser odometry and perform graph optimization to generate a point cloud map. S3. Combine closed-loop thread to perform closed-loop detection and perform global map optimization to finally generate semantic NDT graph; S4. The input LiDAR semantic point cloud is combined with the prior global semantic NDT graph for global localization. After semantic verification, the initial pose is obtained. The subsequent frame-to-graph matching is performed based on the initial pose. Combined with adaptive semantic information constraints, the current LiDAR pose is obtained through coarse matching and fine matching.

2. The semantic mapping and localization system for mobile robots based on 3D LiDAR according to claim 1, characterized in that, In step S2, the planar point features and edge point features in the point cloud are used for registration to obtain the transformation relationship between frames, and then a point cloud map is constructed. The specific construction steps are as follows: pi is the smoothness value of the point to be calculated, and pj are the n nearest points to pi within the same ring. The formula is as follows: For surface features, the semantic weighting calculation follows the same original distance; Based on the smoothness level, edge points and planar points in the point cloud are distinguished according to a threshold and semantic information verification. To ensure uniform distribution of feature points, each ring is divided into Nr sub-regions. Within each region, points are sorted and selected. Finally, Ne points with the lowest smoothness are selected as edge features, and Np points with the highest smoothness are selected as planar features. The resulting edge feature cloud Ce and planar feature cloud Cp are then obtained. The specific formulas are as follows: d ε The distance is the distance from the i-th edge point at time k+1 to the straight line formed by the two edge points j and l at time k. The distance is the distance from the i-th plane point at time k+1 to the plane formed by the three points j, l, and m at time k. As shown in the formula below, F is the overall optimization objective function, d ε and These correspond to the distance from the target point to the edge feature and the distance from the target point to the planar feature, respectively, where w is the semantic weight factor for each target point. The Jacobian of the objective function F is calculated, and the formula is divided into two parts by the chain rule: The first part of the chain rule: where wi is the semantic weight factor, a ε b ε c ε This is the vector perpendicular to the target line from the edge feature. The normal vector of the target plane: The second part of the chain rule involves differentiating the translations in the transformation matrix: Differentiate the rotations in the transformation matrix:

3. The semantic mapping and localization system for mobile robots based on 3D LiDAR according to claim 1, characterized in that, In step S4, lightweight saving and reading are performed using a semantic NDT map. During real-time positioning, the distribution of semantic NDT cells is used to recover local point cloud information. The strategy for generating a semantic NDT map is as follows: semantic NDT cells are dynamically generated based on the semantic information of the point cloud.

4. A semantic mapping and localization system for mobile robots based on 3D LiDAR according to claim 1, characterized in that, In step S4, the localization module uses a normal distribution from distribution to distribution to solve the problem. First, the space is discretized into a voxel distribution, where Sc is the number of point clouds in a single NDT voxel, and p is the set of point clouds in that voxel. The representation of each NDT voxel is as follows: For a 3D point cloud, the probability density expression for each NDT voxel unit is: The formula for calculating the distance between voxels c and l is: Where ucl is the vector distance from uc to ul after transformation: μ cl (p)=Rμ c +t-μ l ; The location is determined using a scan-to-map matching method. This is the current radar frame point cloud set. For the submap point cloud set, It is a sub-map generated based on the radar's current world coordinates and effective scanning range. The sub-map is obtained by resampling the semantic NDT map. c，l Let the semantic information weighting factor be the objective function, and the expression for the objective function be: The gradient vector of the objective function f(T) can be obtained by taking the partial derivative at point p, and the derivative is expressed as follows:

5. A semantic mapping and localization system for mobile robots based on 3D LiDAR according to claim 1, characterized in that, In semantic mapping and localization systems, by calculating the number of points belonging to the same category as the current point and the total number of feature point clouds, the weight reflects the proportion of points from the same category, thereby reducing the mismatch between points from different categories. The formula for this semantic weight is:

6. A semantic mapping and localization system for mobile robots based on 3D LiDAR according to claim 5, characterized in that, The distance residual is incorporated into the semantic weight calculation, and the specific formula is as follows: For surface features, semantic weight calculation follows the same principle, and the distance residual is shown in the formula:

7. A semantic mapping and localization system for mobile robots based on 3D LiDAR according to claim 6, characterized in that, In the process of semantic mapping, the final formula for the adaptive semantic weights of edge features and surface features is:

Images