Semantic driven laser-radar and camera dual-source navigation method and system with point cloud weighting

Abstract

The application belongs to the technical field of robot navigation, and discloses a kind of semantic driving point cloud weighting laser radar and camera dual-source navigation method and system, method includes to the point cloud data and image data of collection data pre-processing, output pre-processed point cloud data and image data;The point cloud data and image data after pre-processing are semantically analyzed, and the point cloud data with semantic label and structured semantic description are obtained;Differentiated weight is assigned to the point cloud data with semantic label and fused structured semantic description, three-dimensional semantic map is constructed, and robot pose data is output;According to three-dimensional semantic map, cost evaluation map is obtained, combined with robot pose data, to generate optimal navigation path.The application is suitable for autonomous navigation task of mobile robot in complex dynamic environment, through dual-source perception fusion of laser radar and camera, combined with semantic confidence and geometric stability coupling differentiated weight distribution mechanism, high-precision real-time positioning and path planning are realized.

Description

Technical Field

[0001] This invention belongs to the field of robot navigation technology and relates to a semantically driven point cloud weighted dual-source navigation method and system using LiDAR and camera. Background Technology

[0002] With the rapid development of Industry 4.0 and intelligent manufacturing, mobile robots are increasingly being used in industrial inspection, warehousing and logistics, and outdoor surveying. Autonomous navigation technology, as the core support for mobile robots, has the key task of achieving high-precision self-localization, environmental mapping, and path planning using sensor data in unknown or complex dynamic environments.

[0003] Currently, single-sensor solutions are insufficient to meet the demands of increasingly complex scenarios. For example, while LiDAR can provide accurate 3D geometric information and is resistant to lighting interference, it is prone to positioning failures in scenarios with degraded geometric features, such as long corridors and glass curtain walls. Visual cameras, while providing rich texture and semantic information, are greatly affected by changes in lighting and have relatively low depth accuracy. Therefore, Simultaneous Localization and Mapping (SLAM) technology based on the fusion of LiDAR and camera sources has become a mainstream research direction in the field of mobile robotics. However, existing SLAM technologies still face many bottlenecks in practical applications.

[0004] Existing SLAM algorithms, such as LOAM and LIO-SAM, typically treat all point cloud data within the field of view equally during point cloud registration (ICP / NDT). In complex scenes containing dynamic obstacles such as pedestrians, forklifts, and temporary cargo, these dynamic feature points are incorrectly included in the pose calculation of rigid body transformations, leading to convergence deviations in the optimization objective function and resulting in positioning drift. Although some methods attempt to remove dynamic points through geometric filtering, traditional geometric filtering is prone to accidentally deleting small features or missing noise points when object edges are blurred or movement is slow, resulting in a decrease in the accuracy and purity of map construction.

[0005] Furthermore, most existing semantic navigation systems only use semantic information for "object recognition" without deeply exploring its contribution to "localization" accuracy. Most semantic segmentation networks typically output hard classification labels directly, ignoring the probabilistic errors inherent in the network's recognition process. When changes in lighting lead to semantic misidentification, the system lacks a self-correction mechanism, resulting in incorrect navigation decisions. Moreover, traditional fusion algorithms fail to establish a mathematical connection between semantic attributes and geometric stability. For example, a smooth concrete wall and a clump of messy bushes are considered to have the same weight in traditional algorithms. This lack of differentiated weighting prevents robots from instinctively relying on stable, reliable landmarks for localization, unlike humans, and from ignoring unstable areas.

[0006] In efficiency-driven scenarios such as industrial inspection, robots are often in high-speed motion. Traditional LiDAR distortion correction methods mostly employ linear interpolation based on discrete timestamps. This method struggles to accurately reproduce the true geometric structure of the environment under non-uniform motion or severe turbulence, resulting in geometric tearing in the input point cloud data itself, further limiting the performance ceiling of high-precision navigation systems.

[0007] In summary, there is an urgent need for a navigation system that can achieve spatiotemporal continuous distortion correction and dynamically allocate point cloud weights based on semantic confidence and geometric stability, in order to solve the problems of insufficient positioning robustness and low mapping accuracy in complex dynamic environments. Summary of the Invention

[0008] The purpose of this invention is to provide a semantically driven point cloud weighted dual-source navigation method using lidar and camera to solve the problems of indiscriminate point cloud processing and separation of semantic and geometric logic in traditional dual-source navigation, and to achieve high-precision and highly adaptable autonomous navigation in complex environments.

[0009] Another objective of this invention is to provide a semantically driven point cloud weighted dual-source navigation system for lidar and camera.

[0010] To achieve the above objectives, the technical solution adopted by this invention is as follows:

[0011] A semantically driven point cloud-weighted dual-source navigation method using LiDAR and camera includes the following steps:

[0012] S1. Collect 3D point cloud data of the environment through LiDAR, collect RGB image data of the environment through camera, perform data preprocessing on point cloud data and image data, and output the preprocessed point cloud data and image data.

[0013] S2. Semantic parsing is performed on the preprocessed point cloud data and image data respectively to identify the object category corresponding to the preprocessed point cloud data, and to extract the landmark text information and spatial mapping coordinates from the preprocessed image data. Finally, point cloud data with semantic labels and a structured semantic description containing the relationship between landmark text and coordinates are output.

[0014] S3. Construct a dynamic evaluation model based on the coupling of semantic confidence and geometric stability, assign differentiated weights to point cloud data with semantic labels, and obtain point cloud data with differentiated weights.

[0015] S4. Integrate point cloud data with differentiated weights with structured semantic descriptions to construct a 3D semantic map. Based on the weight-optimized point cloud registration algorithm, the robot is positioned on the 3D semantic map, and the robot pose data is output.

[0016] S5. Obtain a cost evaluation map for path planning based on the 3D semantic map, and generate the optimal navigation path by combining it with the robot pose data.

[0017] As a limitation, the data preprocessing process for point cloud data and image data in step S1 includes:

[0018] S11. Based on the B-spline curve fitting algorithm, obtain the IMU observation sequence during one frame of lidar scanning; extract the angular acceleration and linear acceleration information from the IMU observation sequence, and construct a continuous pose function with time as the independent variable and six-degree-of-freedom pose as the dependent variable; use the continuous pose function to perform motion compensation and coordinate transformation on the points in the point cloud data at different scanning times to eliminate the motion distortion of the lidar during the scanning cycle, and obtain the distortion-free point cloud data.

[0019] The sampling frequency of the IMU observation sequence is set to be no less than 10 times the scanning frequency of the lidar;

[0020] S12. Construct a dynamic threshold function with strength-distance coupling using adaptive bilateral gradient filtering. When the average distance of the local neighborhood of a point in the distortion-free point cloud data is... And neighborhood curvature When the point is identified as an isolated noise point and removed, the point cloud data after noise removal is obtained.

[0021] S13. Input the image data into the preset image semantic segmentation network and extract the initial semantic confidence at the pixel level; project the noise-removed point cloud data onto the camera's two-dimensional imaging plane to generate an effective depth mask; based on the effective depth mask, set the initial semantic confidence of the pixels of the image data that do not fall within the effective range of the point cloud data projection to zero, and obtain the corrected semantic confidence distribution map as the output preprocessed image data.

[0022] As a further limitation, in step S11, the calculation formula for obtaining the distortion-free point cloud data is as follows:

[0023]

[0024] in, This represents the point after distortion removal. This indicates the scanning time when the lidar scans one frame. Continuous pose function, Indicates the scan time The original measurement point, Indicates will The reference time mapped to this frame Continuous pose function;

[0025] In step S12, the dynamic threshold function of strength-distance coupling for:

[0026]

[0027] in, , , These are all empirical calibration parameters. This represents the maximum effective ranging range of the lidar. To detect distance, The intensity is the reflection intensity.

[0028] As a second limitation, the specific process of step S2 includes:

[0029] S21. Use a point cloud semantic segmentation model to perform semantic parsing on the preprocessed point cloud data and identify the object category corresponding to the preprocessed point cloud data; use an image text detection and recognition model to perform semantic parsing on the preprocessed image data and extract landmark text information and spatial mapping coordinates from the preprocessed image data.

[0030] The object categories include static landmarks, ground structures, and dynamic obstacles;

[0031] S22. Based on the object category corresponding to the preprocessed point cloud data, the landmark text content and spatial mapping coordinates in the preprocessed image data, the unique semantic anchor point of the landmark text content is embedded into the preset global map to eliminate semantic alias ambiguity. Finally, point cloud data with semantic labels and a structured semantic description containing the relationship between landmark text and coordinates are output.

[0032] As a third type of limitation, the specific process of step S3 includes:

[0033] S31. Based on the three-dimensional point cloud semantic segmentation network, determine the probability distribution of each point in the semantically labeled point cloud data belonging to each object category, and obtain the semantic uncertainty entropy value of each point belonging to each object category.

[0034] S32. Calculate the semantic certainty factor for each point based on the object category corresponding to each point in the semantically labeled point cloud data, the preset baseline weight of the point cloud, and the semantic uncertainty entropy value corresponding to each point.

[0035] S33. Local neighborhood sampling is performed on each point in the semantically labeled point cloud data to obtain a local neighborhood point cloud. A covariance matrix is ​​constructed from the local neighborhood point cloud. Eigenvalue decomposition is performed on the covariance matrix to obtain the three largest eigenvalues ​​in the local neighborhood point cloud of each point. Based on the eigenvalues, the local linearity and local flatness of the local neighborhood point cloud of each point are calculated, and the geometric stability factor is calculated.

[0036] S34. Based on the semantic determinism factor and geometric stability factor of each point, construct a dynamic evaluation model based on the coupling of semantic confidence and geometric stability, assign differentiated weights to the point cloud data with semantic labels, and obtain point cloud data with differentiated weights.

[0037] As a further limitation, in step S31, the formula for calculating the semantic uncertainty entropy value of each point belonging to each object category is as follows:

[0038]

[0039] in, For the semantically labeled point cloud data, the first The point belongs to the th point The semantic uncertainty entropy value of object category. For the semantically labeled point cloud data, the first The point belongs to the th point The probability of an object being classified into different categories. The total number of object categories;

[0040] In step S32, the formula for calculating the semantic determinism factor for each point is:

[0041]

[0042] in, For the semantically labeled point cloud data, the first The semantic deterministic factor of each point, For the first point cloud data with semantic labels The object category corresponding to each point is assigned a preset baseline weight to the point cloud. This represents the theoretical maximum entropy value for the object category.

[0043] In step S33, the formula for calculating the geometric stability factor is:

[0044]

[0045]

[0046]

[0047] in, For the semantically labeled point cloud data, the first Geometric stability factor at each point For the semantically labeled point cloud data, the first Local linearity of a point cloud in the local neighborhood of a given point. For the semantically labeled point cloud data, the first Local flatness of the local neighborhood point cloud of a given point. These are the weighting coefficients for planar features, used to adjust... right The intensity of contribution; These are the weighting coefficients for linear features, used for adjustment. right The intensity of contribution; , , For the semantically labeled point cloud data, the first The covariance matrix of each point and its local neighborhood point cloud is constructed, and eigenvalue decomposition is performed to obtain the top three eigenvalues ​​in descending order. The first principal axis direction reflects the greatest dispersion of the local neighborhood point cloud distribution; The second principal axis direction reflects the second largest degree of dispersion in the local neighborhood point cloud distribution; The third principal axis direction is used to reflect the least dispersion of the local neighborhood point cloud distribution. ;

[0048] In step S34, the dynamic evaluation model based on the coupling of semantic confidence and geometric stability is as follows:

[0049]

[0050] in, For the semantically labeled point cloud data, the first Differential weights for each point It is the fusion weighting coefficient of the semantic determinism factor and the geometric stability factor, with a value range of [value range missing]. The constant.

[0051] As a further clarification, the specific process of step S4 includes:

[0052] S41. Integrate point cloud data with differentiated weights with structured semantic descriptions, and construct a three-dimensional semantic map based on a probabilistic voxel hash structure. Each voxel node in the three-dimensional semantic map stores geometric attributes, semantic attributes, and weight history.

[0053] S42. A point cloud registration algorithm based on weight optimization is used, and an error objective function for optimizing pose is constructed according to the differentiated weights to obtain the real-time localization of the robot on the 3D semantic map and output the robot pose data.

[0054] The formula for calculating the objective function of error is:

[0055]

[0056] Where R is the robot's Orthogonal rotation matrix, satisfying , It is the identity matrix; , Let be the translation vector of the robot; These are the 3D coordinates of the target point in the 3D semantic map that corresponds to the distortion-free point cloud data at the current moment; For the preprocessed point cloud data, the first The three-dimensional coordinates of a point in the radar local coordinate system; This represents the total number of valid sampling points participating in pose optimization.

[0057] As a further limitation, step S41 also includes: updating the three-dimensional semantic map;

[0058] Only if the differential weight of the current sampling point If the weight is greater than the first preset weight threshold and the semantic conflict rate with the existing voxels of the 3D semantic map is lower than the preset semantic conflict rate threshold, then map fusion update is performed.

[0059] The first preset weight threshold ranges from 0.4 to 0.8, and the preset semantic conflict rate threshold ranges from 10% to 40%.

[0060] To further specify, the specific process in step S5 is as follows:

[0061] S51. Based on the 3D semantic map, and considering the semantic uncertainty entropy value and differential weight of each point belonging to each object category. This yields a cost evaluation map for path planning;

[0062] In a 3D semantic map, the object at each point is categorized as either a dynamic obstacle or a differentiated weight. Low-confidence regions are designated as high-impedance restricted areas, and each point is classified as a road surface with differentiated weights. The high-confidence region is used as the zero-cost potential field region to obtain a cost evaluation map for path planning;

[0063] S52. Combining robot pose data and a cost evaluation map for path planning, a path planning algorithm is used to generate the optimal navigation path.

[0064] This invention also provides a semantically driven point cloud-weighted dual-source navigation system for LiDAR and camera, used to implement the above-described semantically driven point cloud-weighted dual-source navigation method for LiDAR and camera, comprising:

[0065] The dual-source sensing module is used to collect 3D point cloud data of the environment through LiDAR and RGB image data of the environment through camera. It performs data preprocessing on the point cloud data and image data and outputs the preprocessed point cloud data and image data.

[0066] The semantic segmentation module is used to perform semantic parsing on the preprocessed point cloud data and image data respectively, identify the object category corresponding to the preprocessed point cloud data, and extract the landmark text information and spatial mapping coordinates from the preprocessed image data. Finally, it outputs point cloud data with semantic labels and a structured semantic description containing the relationship between landmark text and coordinates.

[0067] The differentiated weight allocation module is used to construct a dynamic evaluation model based on the coupling of semantic confidence and geometric stability, and to allocate differentiated weights to point cloud data with semantic labels.

[0068] The semantic mapping and localization module is used to fuse point cloud data with differentiated weights with structured semantic descriptions to construct a 3D semantic map. Based on the weight-optimized point cloud registration algorithm, the robot is localized on the 3D semantic map and outputs robot pose data.

[0069] The navigation planning module is used to obtain a cost evaluation map for path planning based on the 3D semantic map, and generate the optimal navigation path by combining the robot pose data.

[0070] The present invention, by adopting the above-described technical solution, achieves the following technical advancements compared to existing technologies:

[0071] (1) This invention innovatively constructs a dynamic evaluation model based on the coupling of semantic confidence and geometric stability, which improves the localization robustness. By fusing semantic determinism factors and geometric stability factors to generate differentiated weights, it breaks the limitation of the traditional dual-source navigation system in the "undifferentiated" processing of point clouds. This invention not only identifies object categories, but also quantifies the contribution of point clouds in localization, which significantly improves the localization robustness and mapping accuracy of mobile robots in complex dynamic environments.

[0072] (2) This invention integrates the geometric accuracy of lidar with the semantic perception advantage of camera, and achieves information complementarity through spatiotemporal synchronization and preprocessing, which not only ensures the comprehensiveness of environmental perception, but also focuses on core effective information through weighted filtering.

[0073] (3) This invention has strong universality and can be adapted to multiple scenarios. The weight allocation rules can be dynamically adjusted according to different scenarios. Only the object category and weight value need to be adapted, without changing the core architecture. It has a wide range of scenario adaptation capabilities.

[0074] In summary, this invention is applicable to autonomous navigation tasks of mobile robots in complex dynamic environments. By fusing dual-source perception of LiDAR and camera, and combining a differentiated weight allocation mechanism that couples semantic confidence and geometric stability, high-precision real-time positioning and path planning can be achieved. Attached Figure Description

[0075] Figure 1The diagram shown is a flowchart of the semantically driven point cloud weighted dual-source navigation method of LiDAR and camera according to Embodiment 1 of the present invention.

[0076] Figure 2 The diagram shown is a structural block diagram of the semantically driven point cloud weighted dual-source navigation system of LiDAR and camera according to Embodiment 2 of the present invention. Detailed Implementation

[0077] To better explain and facilitate understanding of the present invention, the present invention will be described in detail below with reference to the accompanying drawings and specific embodiments.

[0078] Example 1

[0079] like Figure 1 As shown, this embodiment presents a semantically driven point cloud-weighted dual-source navigation method using LiDAR and camera, applied to a mobile robot. The LiDAR is a 32-line mechanical LiDAR with a ranging range of 0.2-120m and an accuracy of [missing information]. cm; the camera uses an industrial-grade RGB-D depth camera with a resolution of 1920×1080 and 30fps. It employs an NVIDIA Jetson AGX Xavier embedded module with 32 TOPS computing power for computation and control, generating microsecond-level ( ) data via an FPGA hardware synchronization board. The ms error trigger signal enables hard synchronization acquisition between the lidar and the camera.

[0080] This embodiment includes the following steps:

[0081] S1. Collect 3D point cloud data of the environment using LiDAR and RGB image data of the environment using a camera. Perform data preprocessing on the point cloud data and image data, and output the preprocessed point cloud data and image data.

[0082] In this step, the data preprocessing of point cloud data and image data includes:

[0083] S11. Based on the B-spline curve fitting algorithm, obtain the IMU observation sequence during one frame of lidar scanning. The sampling frequency of the IMU observation sequence is set to be no less than 10 times the lidar scanning frequency. Extract the angular acceleration and linear acceleration information from the IMU observation sequence to construct a continuous pose function with time as the independent variable and six-degree-of-freedom pose as the dependent variable. :

[0084]

[0085] in, This indicates the scanning time when the lidar scans one frame. Continuous pose function, for B-order spline basis functions, The control point pose is obtained by performing second-order integration of the angular and linear accelerations in the IMU observation sequence and combining it with control point optimization; j is the control point index of the B-spline curve, and m is the maximum boundary value of the control point sequence.

[0086] Using continuous pose functions Calculate the scan time corresponding to each point in the point cloud data. The instantaneous pose is obtained, and motion compensation and coordinate transformation are performed on each point based on the obtained instantaneous pose to eliminate the motion distortion of the lidar during the scanning cycle, and obtain the distortion-free point cloud data.

[0087] The formula for calculating the distortion-free point cloud data is:

[0088]

[0089] in, This represents the point after distortion removal. Indicates the scan time The original measurement point, Indicates will The reference time mapped to this frame Continuous pose function.

[0090] S12. Construct a dynamic threshold function with strength-distance coupling using adaptive bilateral gradient filtering. When the average distance of the local neighborhood of a point in the distortion-free point cloud data is... And neighborhood curvature When the point is identified as an isolated noise point, it is removed to obtain the point cloud data after noise removal.

[0091] In this step, the average local neighborhood distance of a point refers to the average Euclidean distance between that point and its nearest neighbors. Neighborhood curvature is a measure of the degree of curvature of a point on a local neighborhood curve or surface obtained from its nearest neighbors.

[0092] Dynamic threshold function for strength-distance coupling for:

[0093]

[0094] in, , , These are all empirical calibration parameters. , , It can be dynamically adjusted according to the scenario; This represents the maximum effective ranging range of the lidar. To detect distance, The intensity of reflection;

[0095] By constructing a dynamic threshold function with strength-distance coupling It can filter out airborne noise points while preserving object edges and fine geometric features with high reflectivity;

[0096] S13. Input the image data into the preset image semantic segmentation network and extract the initial semantic confidence at the pixel level; project the noise-removed point cloud data onto the camera's two-dimensional imaging plane to generate an effective depth mask; based on the effective depth mask, set the initial semantic confidence of the pixels of the image data that do not fall within the effective range of the point cloud data projection to zero, and obtain the corrected semantic confidence distribution map as the output preprocessed image data.

[0097] The initial semantic confidence is obtained as follows: image data is input into a preset image semantic segmentation network, preferably a semantic segmentation model based on the Transformer architecture, which uses its global self-attention mechanism to extract deep semantic features of the image; the network outputs a probability distribution mapping table of each pixel belonging to each object category through the Softmax activation function at the end of the network. :

[0098]

[0099] in, In the feature map output by the network, the coordinates are... The eigenvector at that location. Take... The maximum probability value of each channel is used as the initial semantic confidence of that pixel. .

[0100] Furthermore, by setting the semantic confidence of pixels that do not fall within the effective range of the point cloud data projection to zero, the aim is to use the physical depth information provided by the LiDAR to perform spatial consistency verification on the visual perception results, thereby eliminating the "out-of-bounds penetration" interference generated by visual semantics at depth abrupt changes (such as object edges), and preventing background textures or noise in the field of view that lack depth support from interfering with subsequent semantic parsing and weight allocation.

[0101] S2. Semantic parsing is performed on the preprocessed point cloud data and image data respectively to identify the object category corresponding to the preprocessed point cloud data, and to extract the landmark text information and spatial mapping coordinates from the preprocessed image data. Finally, point cloud data with semantic labels and a structured semantic description containing the relationship between landmark text and coordinates are output.

[0102] The specific process of this step includes:

[0103] S21. Use the RandLA-Net point cloud semantic segmentation model to perform semantic parsing on the preprocessed point cloud data and identify the object categories corresponding to the preprocessed point cloud data; use the DBNet-CRNN image text detection and recognition model to perform semantic parsing on the preprocessed image data and extract landmark text information and spatial mapping coordinates from the preprocessed image data; object categories include static landmarks, ground structures and dynamic obstacles.

[0104] The process of using the RandLA-Net point cloud semantic segmentation model to perform semantic parsing on preprocessed point cloud data is as follows: The model is pre-trained on a semantically labeled point cloud dataset to extract local spatial geometric features of the point clouds; during the inference phase, the preprocessed point cloud data is input into the network, and the model outputs that each point belongs to a preset... The probability distribution vector of each object category is used to identify the object category.

[0105] The image text detection and recognition process is as follows: A text detection algorithm is used to locate text regions in the image; a recognition algorithm is used to convert pixels within the region into structured strings. The spatial mapping coordinates are obtained by finding the depth value in the effective depth mask generated in step S13, corresponding to the center of the text pixel. By combining the camera intrinsic parameter matrix, the image coordinates are inversely projected to three-dimensional spatial coordinates.

[0106] S22. Based on the object category corresponding to the preprocessed point cloud data, the landmark text content and spatial mapping coordinates in the preprocessed image data, the unique semantic anchor point of the landmark text content is embedded into the preset global map to eliminate semantic alias ambiguity, and the point cloud data with semantic labels and the structured semantic description containing the relationship between landmark text and coordinates are output.

[0107] Step S22 achieves deep alignment between semantic information and spatial location. Here, semantically labeled point cloud data refers to a set of LiDAR sampling points assigned object category attributes, which maps discrete geometric points to abstract object categories; structured semantic description refers to a set of metadata consisting of landmark text content, corresponding spatial mapping coordinates, and globally unique identifiers aligned with semantic anchors, used to support the subsequent topological association and logical indexing of the 3D semantic map.

[0108] S3. Construct a dynamic evaluation model based on the coupling of semantic confidence and geometric stability, assign differentiated weights to point cloud data with semantic labels, and obtain point cloud data with differentiated weights.

[0109] The specific process of this step includes:

[0110] S31. Based on the lightweight PointNet++ 3D point cloud semantic segmentation network, determine the probability distribution of each point in the semantically labeled point cloud data belonging to each object category, and obtain the semantic uncertainty entropy value of each point belonging to each object category.

[0111] In this embodiment, semantically labeled point cloud data is input into a lightweight PointNet++ 3D point cloud semantic segmentation network. This network extracts local geometric features of the point cloud through cascaded ensemble abstraction layers, and uses a feature propagation layer to restore the features to the original point cloud scale through distance-weighted interpolation, thereby obtaining a high-dimensional semantic feature vector for each point.

[0112] Subsequently, the high-dimensional semantic feature vector of each point is mapped to a preset object category space using the fully connected layer at the end of the network, and the probability distribution of each point belonging to each object category in the semantically labeled point cloud data is calculated using the Softmax activation function. Finally, based on the probability distribution The corresponding semantic uncertainty entropy value is calculated using the information entropy formula. :

[0113] The formula for calculating the semantic uncertainty entropy value of each point belonging to each object category is:

[0114]

[0115] in, For the semantically labeled point cloud data, the first The point belongs to the th point The semantic uncertainty entropy value of object category. For the semantically labeled point cloud data, the first The point belongs to the th point The probability of an object being classified into different categories. The total number of preset object categories; when A high value indicates that the network is "hesitant" in classifying the object category, and this information will be used for subsequent weight downgrading.

[0116] The entropy value As an input variable for calculating the semantic certainty factor in step S32, it is used to characterize the confidence level of the semantic attribute of that point.

[0117] S32. Calculate the semantic certainty factor for each point based on the object category corresponding to each point in the semantically labeled point cloud data, the preset baseline weight of the point cloud, and the semantic uncertainty entropy value corresponding to each point.

[0118] Based on the object category to which each point in the semantically labeled point cloud data belongs, the corresponding object category baseline weight is matched from a pre-defined weight mapping table. And combined with the semantic uncertainty entropy value of this point and maximum entropy The semantic determinism factor of each point cloud point is obtained through the following calculation formula. ;

[0119]

[0120] in, For the semantically labeled point cloud data, the first The semantic deterministic factor of each point, For the first point cloud data with semantic labels The object category (static landmark, ground structure, and dynamic obstacle) corresponding to each point has a preset baseline weight on the point cloud, with a value range of... ; This represents the theoretical maximum entropy value for each object category. The system is in a state of maximum uncertainty when the model predicts equal probabilities for each object category. Approaching .

[0121] In this step, static landmarks have fixed locations and strong positioning constraints, serving as the core reference for navigation and positioning. Therefore, static landmarks are given the highest reference weight when set. The ground structure forms the environmental spatial framework and provides basic positioning references. Its positioning value is moderate, therefore it is assigned a moderate reference weight when setting the ground structure. Dynamic obstacles are prone to changing positions, which not only fail to provide stable positioning support but may also interfere with registration accuracy. Therefore, dynamic obstacles should be set with the lowest reference weight. .

[0122] When the confidence level of semantic recognition is extremely high It fully preserves the location value of the object category; when the semantic recognition confidence is low ( near )hour, Significant attenuation occurs; even static landmarks will have their weight reduced due to blurred recognition, thus avoiding positioning interference caused by misidentification.

[0123] S33. Local neighborhood sampling is performed on each point in the semantically labeled point cloud data to obtain a local neighborhood point cloud. A covariance matrix is ​​constructed from the local neighborhood point cloud. Eigenvalue decomposition is performed on the covariance matrix to obtain the three largest eigenvalues ​​in the local neighborhood point cloud of each point. Based on the eigenvalues, the local linearity and local flatness of the local neighborhood point cloud of each point are calculated, and the geometric stability factor is calculated.

[0124] The formula for calculating the geometric stability factor is:

[0125]

[0126]

[0127]

[0128] in, For the semantically labeled point cloud data, the first Geometric stability factor at each point For the semantically labeled point cloud data, the first Local linearity of a point cloud in the local neighborhood of a given point. For the semantically labeled point cloud data, the first Local flatness of the local neighborhood point cloud of a given point. These are the weighting coefficients for planar features, used to adjust... right The intensity of contribution; These are the weighting coefficients for linear features, used for adjustment. right The intensity of contribution; , , For the semantically labeled point cloud data, the first The covariance matrix of each point and its local neighborhood point cloud is constructed, and eigenvalue decomposition is performed to obtain the top three eigenvalues ​​in descending order. The first principal axis direction reflects the greatest dispersion of the local neighborhood point cloud distribution; The second principal axis direction reflects the second largest degree of dispersion in the local neighborhood point cloud distribution; The third principal axis direction is used to reflect the least dispersion of the local neighborhood point cloud distribution. The relative magnitude of the eigenvalues ​​directly reflects the geometric distribution of the local neighborhood point cloud.

[0129] In this step, local linearity Used to quantify whether a local neighborhood point cloud approximates a "linear shape" such as the edge of a pillar or the corner of a wall; local flatness Used to quantify whether a local neighborhood point cloud approximates a "planar shape" like a wall or ground; the eigenvalues ​​of random scattered points without obvious planar or linear features satisfy... ≈ ≈ ,at this time and All approach 0, geometric stability factor The weights automatically decay to a lower value, thus suppressing the weights of invalid point clouds.

[0130] and As an empirical calibration parameter, its value can be dynamically adjusted according to the scenario. The default value is 1.0 for industrial inspection scenarios, while it can be adjusted to 0.9 for complex outdoor scenarios to reduce the weight of geometric features. This ensures that points with significant geometric features (flat walls, straight columns) receive higher weights, while the weights of edges of cluttered dynamic objects are reduced.

[0131] S34. Based on the semantic determinism factor and geometric stability factor of each point, construct a dynamic evaluation model based on the coupling of semantic confidence and geometric stability, assign differentiated weights to the point cloud data with semantic labels, and obtain point cloud data with differentiated weights.

[0132] The dynamic evaluation model based on the coupling of semantic confidence and geometric stability is as follows:

[0133]

[0134] in, For the first Differential weights for each point It is the fusion weighting coefficient of the semantic determinism factor and the geometric stability factor, with a value range of [value range missing]. A constant is used to adjust the contribution ratio of semantic and geometric information in the final weight. hour, The differentiation weights are entirely determined by semantic information; when hour, The differential weights are entirely determined by geometric information; when At that time, the differential weight is a linear weighted fusion of the two. The larger the value, the stronger the influence of semantic information on the final weight.

[0135] In this step, the static landmark point cloud... Corresponding to the first weight, the ground structure point cloud Corresponding to the second weight, the dynamic obstacle point cloud The corresponding third weight, and the first weight > the second weight > the third weight.

[0136] S4. Integrate point cloud data with differentiated weights with structured semantic descriptions to construct a 3D semantic map. Based on the weight-optimized point cloud registration algorithm, the robot is positioned on the 3D semantic map, and the robot pose data is output.

[0137] The specific process of this step includes:

[0138] S41. Integrate point cloud data with differentiated weights with structured semantic descriptions, and construct a three-dimensional semantic map based on a probabilistic voxel hash structure. Each voxel node in the three-dimensional semantic map stores geometric attributes, semantic attributes, and weight history.

[0139] S42. A point cloud registration algorithm based on weight optimization is used, and an error objective function for optimizing pose is constructed according to the differentiated weights to obtain the real-time localization of the robot on the 3D semantic map and output the robot pose data.

[0140] In this step, the formula for calculating the objective function of the error is:

[0141]

[0142] Where R is the robot's Orthogonal rotation matrix, satisfying The value range is in three-dimensional rotation space. , used to describe spatial attitude transformation It is the identity matrix; , This is the robot's translation vector, and its range of values ​​is determined by the robot's motion space. For the preprocessed point cloud data, the first The three-dimensional coordinates of a point in the radar local coordinate system; The total number of valid sampling points participating in pose optimization; For the first Differential weights for each point; It refers to the 3D coordinates of the target point in the 3D semantic map that corresponds to the distortion-free point cloud data at the current moment.

[0143] Under this optimization objective, high-weighted point clouds will dominate pose calculation, while error terms generated by dynamic obstacles will be automatically suppressed, thereby achieving anti-interference localization without the need to remove point clouds.

[0144] In this step, after constructing the error objective function for optimizing the pose, the specific computational path for the robot's real-time localization on the 3D semantic map is obtained as follows: the system defines the solution problem in the manifold space corresponding to the Lie group / Lie algebra. The Gauss-Newton iterative algorithm is used to perform nonlinear optimization of the error objective function. By continuously minimizing the weighted Euclidean residuals, the state variables (orthogonal rotation matrix) are optimized. With translation vector The system performs incremental updates. When the residual converges to a preset threshold, it outputs the optimal robot pose data for the current moment. This process ensures the real-time performance and robustness of the localization results in complex environments by relying on high-weight point clouds and suppressing low-weight dynamic points.

[0145] Step S41 also includes: updating the three-dimensional semantic map;

[0146] Only if the differential weight of the current sampling point If the weight is greater than the first preset weight threshold and the semantic conflict rate with the existing voxels of the 3D semantic map is lower than the preset semantic conflict rate threshold, then map fusion update is performed.

[0147] In this step, a first preset weight threshold is used to filter high-reliability feature points participating in map construction. The preferred value range for this threshold is 0.4 to 0.8. The logic behind selecting this range is that if the threshold is below 0.4, unstable dynamic objects or highly uncertain semantic point clouds in the environment are easily incorporated into the map, leading to "ghosting" or decreased accuracy. If the threshold is above 0.8, the map update conditions are too stringent, potentially resulting in ineffective map completion in sparsely populated areas. This embodiment ensures the geometric accuracy and semantic robustness of the 3D semantic map by setting this threshold. Furthermore, a preset semantic conflict rate threshold is used to measure the consistency between current observations and historical map data, preventing semantic pollution caused by misidentification. The preferred value range for this threshold is 10% to 40%. The logic behind selecting this range is that the semantic conflict rate reflects the degree of mismatch between old and new labels within a voxel node. A lower threshold (e.g., 10%) ensures the absolute purity of the map's semantics, suitable for closed environments with extremely high navigation accuracy requirements; a higher threshold (e.g., 40%) gives the system stronger adaptability to environmental evolution. This embodiment employs the aforementioned threshold discrimination mechanism, enabling smooth updates and error self-correction of the semantic map in dynamically changing environments.

[0148] S5. Obtain a cost evaluation map for path planning based on the 3D semantic map, combine it with the robot pose data, generate the optimal navigation path and output motion control commands.

[0149] The specific process in step S5 is as follows:

[0150] S51. Based on the three-dimensional semantic map, determine the semantic uncertainty entropy value of each point belonging to each object category. and differentiated weights This yields a cost evaluation map for path planning;

[0151] In a 3D semantic map, the object at each point is categorized as either a dynamic obstacle or a differentiated weight. Low-confidence regions are designated as high-impedance restricted areas, and each point is classified as a road surface with differentiated weights. The high-confidence region is used as the zero-cost potential field region to obtain a cost evaluation map for path planning;

[0152] S52. Combining robot pose data and cost evaluation map for path planning, the optimal navigation path is generated using a hybrid A* algorithm, and motion control commands are output to drive the robot to travel along a high-weight semantic region, thus completing closed-loop control from sensory input to physical execution.

[0153] Example 2

[0154] This embodiment describes a semantically driven point cloud-weighted dual-source navigation system using LiDAR and a camera. Figure 2 The diagram shown is a block diagram of a semantically driven point cloud-weighted dual-source navigation system for lidar and camera, provided according to the method of Embodiment 1, including:

[0155] The dual-source sensing module is used to collect 3D point cloud data of the environment through LiDAR and RGB image data of the environment through camera. It performs data preprocessing on the point cloud data and image data and outputs the preprocessed point cloud data and image data.

[0156] The semantic segmentation module is used to perform semantic parsing on the preprocessed point cloud data and image data respectively, identify the object category corresponding to the preprocessed point cloud data, and extract the landmark text information and spatial mapping coordinates from the preprocessed image data. Finally, it outputs point cloud data with semantic labels and a structured semantic description containing the relationship between landmark text and coordinates.

[0157] The differentiated weight allocation module is used to construct a dynamic evaluation model based on the coupling of semantic confidence and geometric stability, and to allocate differentiated weights to point cloud data with semantic labels.

[0158] The semantic mapping and localization module is used to fuse point cloud data with differentiated weights with structured semantic descriptions to construct a 3D semantic map. Based on the weight-optimized point cloud registration algorithm, the robot is localized on the 3D semantic map and outputs robot pose data.

[0159] The navigation planning module is used to obtain a cost evaluation map for path planning based on the 3D semantic map, combine it with robot pose data, generate the optimal navigation path, and output motion control commands.

[0160] In this embodiment, the semantically driven point cloud weighted LiDAR and camera dual-source navigation system is only illustrated by the above-mentioned functional module division when performing data processing. In actual applications, the above functions can be assigned to different functional modules as needed.

[0161] It should be noted that the above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention. Although the present invention has been described in detail with reference to the above embodiments, those skilled in the art can still modify the technical solutions described in the above embodiments or make equivalent substitutions for some of the technical features. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A semantically driven point cloud-weighted dual-source navigation method for lidar and camera, characterized in that, Includes the following steps: S1. Collect 3D point cloud data of the environment through LiDAR, collect RGB image data of the environment through camera, perform data preprocessing on point cloud data and image data, and output the preprocessed point cloud data and image data. S2. Semantic parsing is performed on the preprocessed point cloud data and image data respectively to identify the object category corresponding to the preprocessed point cloud data, and to extract the landmark text information and spatial mapping coordinates from the preprocessed image data. Finally, point cloud data with semantic labels and a structured semantic description containing the relationship between landmark text and coordinates are output. S3. Construct a dynamic evaluation model based on the coupling of semantic confidence and geometric stability, assign differentiated weights to point cloud data with semantic labels, and obtain point cloud data with differentiated weights. S4. Integrate point cloud data with differentiated weights with structured semantic descriptions to construct a 3D semantic map. Based on the weight-optimized point cloud registration algorithm, the robot is positioned on the 3D semantic map, and the robot pose data is output. S5. Obtain a cost evaluation map for path planning based on the 3D semantic map, and generate the optimal navigation path by combining it with the robot pose data.

2. The semantically driven point cloud-weighted dual-source navigation method for lidar and camera according to claim 1, characterized in that, Step S1, the data preprocessing process for point cloud data and image data includes: S11. Based on the B-spline curve fitting algorithm, obtain the IMU observation sequence during one frame of lidar scanning; extract the angular acceleration and linear acceleration information from the IMU observation sequence, and construct a continuous pose function with time as the independent variable and six-degree-of-freedom pose as the dependent variable; use the continuous pose function to perform motion compensation and coordinate transformation on the points in the point cloud data at different scanning times to eliminate the motion distortion of the lidar during the scanning cycle, and obtain the distortion-free point cloud data. The sampling frequency of the IMU observation sequence is set to be no less than 10 times the scanning frequency of the lidar; S12. Construct a dynamic threshold function with strength-distance coupling using adaptive bilateral gradient filtering. When the average distance of the local neighborhood of a point in the distortion-free point cloud data is... And neighborhood curvature When the point is identified as an isolated noise point and removed, the point cloud data after noise removal is obtained. S13. Input the image data into the preset image semantic segmentation network and extract the initial semantic confidence at the pixel level; project the noise-removed point cloud data onto the camera's two-dimensional imaging plane to generate an effective depth mask; based on the effective depth mask, set the initial semantic confidence of the pixels of the image data that do not fall within the effective range of the point cloud data projection to zero, and obtain the corrected semantic confidence distribution map as the output preprocessed image data.

3. The semantically driven point cloud-weighted dual-source navigation method for lidar and camera according to claim 2, characterized in that, In step S11, the calculation formula for the distortion-free point cloud data is as follows: in, This represents the point after distortion removal. This indicates the scanning time when the lidar scans one frame. Continuous pose function, Indicates the scan time The original measurement point, Indicates will The reference time mapped to this frame Continuous pose function; In step S12, the dynamic threshold function of strength-distance coupling for: in, , , These are all empirical calibration parameters. This represents the maximum effective ranging range of the lidar. To detect distance, The intensity is the reflection intensity.

4. The semantically driven point cloud-weighted dual-source navigation method for lidar and camera according to claim 1, characterized in that, The specific process of step S2 includes: S21. Use a point cloud semantic segmentation model to perform semantic parsing on the preprocessed point cloud data and identify the object category corresponding to the preprocessed point cloud data; use an image text detection and recognition model to perform semantic parsing on the preprocessed image data and extract landmark text information and spatial mapping coordinates from the preprocessed image data. The object categories include static landmarks, ground structures, and dynamic obstacles; S22. Based on the object category corresponding to the preprocessed point cloud data, the landmark text content and spatial mapping coordinates in the preprocessed image data, the unique semantic anchor point of the landmark text content is embedded into the preset global map to eliminate semantic alias ambiguity. Finally, point cloud data with semantic labels and a structured semantic description containing the relationship between landmark text and coordinates are output.

5. The semantically driven point cloud-weighted dual-source navigation method for lidar and camera according to claim 1, characterized in that, The specific process of step S3 includes: S31. Based on the three-dimensional point cloud semantic segmentation network, determine the probability distribution of each point in the semantically labeled point cloud data belonging to each object category, and obtain the semantic uncertainty entropy value of each point belonging to each object category. S32. Calculate the semantic certainty factor for each point based on the object category corresponding to each point in the semantically labeled point cloud data, the preset baseline weight of the point cloud, and the semantic uncertainty entropy value corresponding to each point. S33. Local neighborhood sampling is performed on each point in the semantically labeled point cloud data to obtain a local neighborhood point cloud. A covariance matrix is ​​constructed from the local neighborhood point cloud. Eigenvalue decomposition is performed on the covariance matrix to obtain the three largest eigenvalues ​​in the local neighborhood point cloud of each point. Based on the eigenvalues, the local linearity and local flatness of the local neighborhood point cloud of each point are calculated, and the geometric stability factor is calculated. S34. Based on the semantic determinism factor and geometric stability factor of each point, construct a dynamic evaluation model based on the coupling of semantic confidence and geometric stability, assign differentiated weights to the point cloud data with semantic labels, and obtain point cloud data with differentiated weights.

6. The semantically driven point cloud-weighted dual-source navigation method for lidar and camera according to claim 5, characterized in that, In step S31, the formula for calculating the semantic uncertainty entropy value of each point belonging to each object category is as follows: in, For the semantically labeled point cloud data, the first The point belongs to the th point The semantic uncertainty entropy value of object category. For the semantically labeled point cloud data, the first The point belongs to the th point The probability of an object being classified into different categories. The total number of object categories; In step S32, the formula for calculating the semantic determinism factor for each point is: in, For the semantically labeled point cloud data, the first The semantic deterministic factor of each point, For the first point cloud data based on semantic labels The object category corresponding to each point is assigned a preset baseline weight to the point cloud. This represents the theoretical maximum entropy value for the object category. In step S33, the formula for calculating the geometric stability factor is: in, For the semantically labeled point cloud data, the first Geometric stability factor at each point For the semantically labeled point cloud data, the first Local linearity of a point cloud in its local neighborhood. For the semantically labeled point cloud data, the first Local flatness of the local neighborhood point cloud of a point. These are the weighting coefficients for planar features, used to adjust... right The intensity of contribution; These are the weighting coefficients for linear features, used to adjust... right The intensity of contribution; , , For the semantically labeled point cloud data, the first The covariance matrix of each point and its local neighborhood point cloud is constructed, and eigenvalue decomposition is performed to obtain the top three eigenvalues ​​in descending order. The first principal axis direction reflects the greatest dispersion of the local neighborhood point cloud distribution; The second principal axis direction reflects the second largest degree of dispersion in the local neighborhood point cloud distribution; The third principal axis direction is used to reflect the least dispersion of the local neighborhood point cloud distribution. ; In step S34, the dynamic evaluation model based on the coupling of semantic confidence and geometric stability is as follows: in, For the semantically labeled point cloud data, the first Differential weights for each point It is the fusion weighting coefficient of the semantic determinism factor and the geometric stability factor, with a value range of [value range missing]. The constant.

7. The semantically driven point cloud-weighted dual-source navigation method for lidar and camera according to claim 6, characterized in that, The specific process of step S4 includes: S41. Integrate point cloud data with differentiated weights with structured semantic descriptions, and construct a three-dimensional semantic map based on a probabilistic voxel hash structure. Each voxel node in the three-dimensional semantic map stores geometric attributes, semantic attributes, and weight history. S42. A point cloud registration algorithm based on weight optimization is used, and an error objective function for optimizing pose is constructed according to the differentiated weights to obtain the real-time localization of the robot on the 3D semantic map and output the robot pose data. The formula for calculating the objective function of error is: Where R is the robot's Orthogonal rotation matrix, satisfying , It is the identity matrix; , Let be the translation vector of the robot; These are the 3D coordinates of the target point in the 3D semantic map that corresponds to the distortion-free point cloud data at the current moment; For the preprocessed point cloud data, the first The three-dimensional coordinates of a point in the radar local coordinate system; This represents the total number of valid sampling points participating in pose optimization.

8. The semantically driven point cloud-weighted dual-source navigation method for lidar and camera according to claim 7, characterized in that, Step S41 also includes: updating the three-dimensional semantic map; Only if the differential weight of the current sampling point If the weight is greater than the first preset weight threshold and the semantic conflict rate with the existing voxels of the 3D semantic map is lower than the preset semantic conflict rate threshold, then map fusion update is performed. The first preset weight threshold ranges from 0.4 to 0.8, and the preset semantic conflict rate threshold ranges from 10% to 40%.

9. The semantically driven point cloud-weighted dual-source navigation method for lidar and camera according to claim 7, characterized in that, The specific process in step S5 is as follows: S51. Based on the 3D semantic map, and considering the semantic uncertainty entropy value and differential weight of each point belonging to each object category. This yields a cost evaluation map for path planning; In a 3D semantic map, the object at each point is categorized as either a dynamic obstacle or a differentiated weight. Low-confidence regions are designated as high-impedance restricted areas, and each point is classified as a road surface with differentiated weights. The high-confidence region is used as the zero-cost potential field region to obtain a cost evaluation map for path planning; S52. Combining robot pose data and a cost evaluation map for path planning, a path planning algorithm is used to generate the optimal navigation path.

10. A semantically driven point cloud-weighted dual-source navigation system for lidar and camera, used to implement the semantically driven point cloud-weighted dual-source navigation method for lidar and camera as described in any one of claims 1 to 9, characterized in that, include: The dual-source sensing module is used to collect 3D point cloud data of the environment through LiDAR and RGB image data of the environment through camera. It performs data preprocessing on the point cloud data and image data and outputs the preprocessed point cloud data and image data. The semantic segmentation module is used to perform semantic parsing on the preprocessed point cloud data and image data respectively, identify the object category corresponding to the preprocessed point cloud data, and extract the landmark text information and spatial mapping coordinates from the preprocessed image data. Finally, it outputs point cloud data with semantic labels and a structured semantic description containing the relationship between landmark text and coordinates. The differentiated weight allocation module is used to construct a dynamic evaluation model based on the coupling of semantic confidence and geometric stability, and to allocate differentiated weights to point cloud data with semantic labels. The semantic mapping and localization module is used to fuse point cloud data with differentiated weights with structured semantic descriptions to construct a 3D semantic map. Based on the weight-optimized point cloud registration algorithm, the robot is localized on the 3D semantic map and outputs robot pose data. The navigation planning module is used to obtain a cost evaluation map for path planning based on the 3D semantic map, and generate the optimal navigation path by combining the robot pose data.

Images