A multi-sensor fusion SLAM method and system based on comprehensive data quality evaluation

Abstract

The present invention relates to the technical field of synchronous positioning and map construction, and discloses a multi-sensor fusion SLAM method and system based on comprehensive data quality evaluation, through multi-dimensional quality evaluation of laser radar point cloud data and camera image data, the evaluation indicators include the scene degradation degree, field of view integrity, intensity information entropy of the point cloud, as well as the clarity, self-similarity and natural quality of the image, and the subjective and objective weight fusion method is used to calculate the comprehensive weight of each indicator, and according to the comprehensive quality score of the point cloud data and the image data, the laser radar posture, the visual sensor posture or the IMU integral posture increment is dynamically selected as the initial value, and the matching optimization of the point cloud and the map and the global posture graph optimization are performed. The present invention significantly improves the positioning accuracy and robustness of the SLAM system in complex environments by real-time evaluation of sensor data quality and dynamic adjustment of the fusion strategy, while reducing the computational complexity.

Description

Technical Field

[0001] The present invention relates to the technical field of synchronous positioning and map construction, and in particular to a multi-sensor fusion SLAM method and device based on comprehensive data quality evaluation. Background Art

[0002] With the rapid development of robotics and autonomous driving, multi-sensor fusion SLAM (Simultaneous Localization and Mapping) technology has become a key technology for realizing autonomous navigation of intelligent systems. This technology can effectively improve the positioning accuracy of the system by fusing multiple sensor data.

[0003] At present, the mainstream multi-sensor fusion SLAM algorithms mainly adopt two strategies: one is to use the pose estimation result of the visual subsystem as the initial value of the lidar ScantoMap; the other is to filter and fuse the pose estimation results of the vision and lidar. Representative algorithms include LVI-SAM, FAST-LIVO, etc. These methods can achieve good results under ideal conditions. However, the existing technologies generally lack a real-time evaluation mechanism for sensor data quality. In practical applications, the quality of sensor data will change dynamically due to various factors. For example, image data will be affected by illumination changes, occlusion, motion blur, etc., and the quality of lidar data may decrease due to environmental degradation, sparse point clouds, etc. At the same time, the existing methods use a fixed fusion strategy and fail to adjust dynamically according to the data quality. When the quality of a certain sensor data decreases significantly, continuing to use the preset fusion method may lead to a decrease in system accuracy, and even cause positioning failure in severe cases. In addition, the existing methods often rely on a single indicator or empirical threshold to judge data quality, lack a comprehensive evaluation of sensor reliability in different scenarios, and cannot fully reflect the actual performance of sensors in complex environments.

[0004] Therefore, how to achieve real-time comprehensive evaluation of sensor data quality and dynamically adjust the fusion strategy accordingly to improve the robustness and accuracy of the SLAM system in complex environments is a key technical problem that needs to be solved urgently. Summary of the invention

[0005] In order to overcome the defects of the prior art, the present invention proposes a multi-sensor fusion SLAM method and system based on comprehensive data quality evaluation, aiming to solve the problem that the prior art does not fully consider the impact of sensor data quality on the performance of the SLAM system. By multi-dimensional quality evaluation of image and point cloud data, the pose estimation strategy is dynamically adjusted to improve the accuracy and robustness of the SLAM system in complex environments.

[0006] To achieve the above object, on the one hand, the present invention provides a multi-sensor fusion SLAM method based on comprehensive evaluation of data quality, comprising:

[0007] Acquire LiDAR point cloud data, visual sensor image data, and inertial measurement unit (IMU) data;

[0008] Perform pre-integration processing on the IMU data to obtain the IMU integral pose increment, and calculate the pose estimation value based on the lidar and the pose estimation value based on the visual sensor at the current moment based on the point cloud data and image data respectively;

[0009] The quality of point cloud data and image data is evaluated based on multi-dimensional quality evaluation indicators, including:

[0010] Point cloud data quality assessment indicators include scene degradation, field of view completeness, and intensity information entropy;

[0011] Image data quality assessment indicators include image clarity, self-similarity, and natural quality;

[0012] A weight calculation method based on the fusion of subjective weight and objective weight is used to determine the weight and comprehensive weight of each quality assessment indicator, and calculate the comprehensive quality score of point cloud data and image data;

[0013] According to the comprehensive quality scores of point cloud data and image data, dynamically select the pose estimation value based on lidar, the pose estimation value based on visual sensor or the IMU integrated pose increment as the optimization initial value;

[0014] Based on the selected optimization initial value, the matching optimization between the point cloud and the map and the global pose graph optimization are performed to obtain the optimal pose estimation value and point cloud map.

[0015] As a multi-sensor fusion SLAM method based on comprehensive data quality evaluation of the present invention, further, the step of calculating the pose estimation value based on the laser radar includes: arranging the original point cloud data according to the laser radar scanning line number, performing corner point and surface point feature detection, outputting corner point cloud and surface point point cloud, and calculating the pose transformation from the current frame to the previous frame based on the corner point cloud and the surface point cloud to obtain the pose estimation value based on the laser radar.

[0016] As a multi-sensor fusion SLAM method based on comprehensive data quality evaluation of the present invention, further, the step of calculating the pose estimation value based on the visual sensor includes: extracting features from the original image data to obtain feature key points and feature descriptor information, and matching the features between the current image frame and the previous frame; constructing and solving the epipolar constraint problem based on matching feature point pairs, and initializing the three-dimensional feature map; calculating the pose transformation between the current image frame and the previous frame using a pose solving algorithm; using a sliding window method to construct a local BA optimization problem, and jointly optimizing the poses of all frames in the local window with the map points to obtain the pose estimation value based on the visual sensor.

[0017] As a multi-sensor fusion SLAM method based on comprehensive data quality evaluation of the present invention, further, the calculation method of the point cloud data quality evaluation index includes:

[0018] The calculation method of the point cloud data quality evaluation index includes:

[0019] The degree of scene degradation is to calculate the angle between the normal vectors of each point in the point cloud and its K nearest neighboring points. If there is an angle greater than the preset angle threshold, it is determined to be a valid point. The ratio of the number of valid points to the total number of points in the point cloud is calculated to obtain the degree of scene degradation.

[0020] Field of view completeness: calculate the ratio of the actual horizontal field of view to the theoretical horizontal field of view of the LiDAR sensor design to get the horizontal field of view completeness, and calculate the ratio of the actual vertical field of view to the theoretical vertical field of view of the LiDAR sensor design to get the vertical field of view completeness; take the minimum value of the horizontal field of view completeness and the vertical field of view completeness as the point cloud field of view completeness;

[0021] Intensity information entropy, the intensity values ​​of all points in the point cloud are counted, the probability distribution entropy of the intensity distribution is calculated, and the entropy value is normalized to obtain the point cloud intensity information entropy.

[0022] As a multi-sensor fusion SLAM method based on comprehensive data quality evaluation of the present invention, further, the calculation method of the image data quality evaluation index includes:

[0023] Image clarity: for each pixel in the image, a processing area with a preset window size is designed with the current pixel as the center, and the local contrast is calculated; the number of non-flat points and high-contrast points is counted, and the weighted sum is calculated as the image clarity;

[0024] Self-similarity: divide the image into tiles of the same size, use the perceptual hashing algorithm to calculate the similarity between tiles, count the proportion of similar tiles, and obtain the image self-similarity;

[0025] Natural quality,extracts characteristic coefficients from the image, fits them into statistical distribution features,and regresses them through a machine learning model to obtain the natural quality of the image.

[0026] As a multi-sensor fusion SLAM method based on comprehensive data quality evaluation of the present invention, further, the calculation method of the weight and comprehensive weight of each quality evaluation index includes:

[0027] According to the preset sample sampling duration and sample sampling quantity, the point cloud data quality assessment results and the image data quality assessment results are evenly sampled within the sampling time range to obtain the point cloud data quality assessment sample list and the image data quality assessment sample list respectively; the subjective ranking method is used to determine the subjective weight of each quality assessment indicator; the entropy weight method is used to standardize the sample data and calculate the information entropy to determine the objective weight of each quality assessment indicator; the weighted fusion method is used to comprehensively calculate the subjective weight and the objective weight to obtain the comprehensive weight.

[0028] As a multi-sensor fusion SLAM method based on comprehensive data quality evaluation of the present invention, further, the matching optimization step of the point cloud and the map includes:

[0029] Based on the dynamically selected pose estimation value, key frames are found within the preset radius to build a local map;

[0030] Match the current frame point cloud with the local map and optimize the pose;

[0031] Determine whether to set the current frame as a key frame based on the change in posture.

[0032] As a multi-sensor fusion SLAM method based on comprehensive data quality evaluation of the present invention, further, the step of optimizing the global pose graph includes:

[0033] Perform pre-integration calculation based on IMU data;

[0034] Use loop detection technology to detect the loop relationship between the current frame and historical key frames;

[0035] The key frame pose, IMU pre-integration value, and loop detection results are used as prior and constraint factors to construct a factor graph optimization problem.

[0036] A nonlinear optimization algorithm is used to optimize the factor graph to obtain globally consistent pose estimation results.

[0037] As a multi-sensor fusion SLAM method based on comprehensive data quality evaluation of the present invention, further, a method for dynamically selecting and optimizing initial values ​​is:

[0038] When the comprehensive evaluation result of the point cloud data quality is greater than the preset threshold, the pose estimation value based on the lidar sensor is selected as the optimization initial value;

[0039] When the comprehensive evaluation result of the image data quality is greater than the preset threshold, the pose estimation value based on the visual sensor is selected as the optimization initial value;

[0040] Otherwise, the IMU integral pose increment is selected as the initial value for optimization.

[0041] On the other hand, the present invention provides a multi-sensor fusion SLAM system based on comprehensive data quality evaluation, comprising:

[0042] IMU pre-integration module: receives IMU data, performs pre-integration based on IMU data, calculates IMU integral posture increment, and outputs it to the data quality comprehensive evaluation module;

[0043] LiDAR odometer subsystem, which includes:

[0044] Point cloud preprocessing module: used to receive point cloud data collected by the laser radar in real time, perform corner point and surface point feature detection, and output corner point and surface point feature point cloud;

[0045] Inter-frame odometry module: used to receive feature point clouds, calculate the pose transformation between the current point cloud frame and the previous frame based on corner point and surface point features, output the pose estimation value based on the lidar, and pass the estimated pose information to the data quality comprehensive evaluation module;

[0046] Visual odometry subsystem, which includes:

[0047] Feature extraction and matching module: used to receive image data collected by the visual sensor in real time, extract feature key points and feature descriptor information, and perform feature matching between image frames;

[0048] Image frame pose estimation module: used to construct and solve the epipolar constraint problem based on matching feature point pairs, initialize the three-dimensional feature map, calculate the pose transformation between image frames using the pose solving algorithm, and pass the estimated pose information to the local BA optimization module;

[0049] Local BA optimization module: The sliding window method is used to construct the local BA optimization problem, the poses and map points of all frames in the local window are jointly optimized, and the optimized pose estimation values ​​based on the visual sensor are passed to the comprehensive data quality evaluation module;

[0050] The data quality multi-index comprehensive evaluation subsystem includes:

[0051] Point cloud data quality assessment module: multi-dimensional quality assessment of point cloud data based on scene degradation, field of view integrity and intensity information entropy;

[0052] Image data quality assessment module: performs multi-dimensional quality assessment of image data based on image clarity, self-similarity and natural quality;

[0053] Data quality comprehensive evaluation module: Dynamically select the pose estimation value based on lidar, the pose estimation value based on visual sensor or the IMU integral pose increment as the optimization initial value, where:

[0054] When the comprehensive evaluation result of the point cloud data quality is greater than the preset threshold, the pose estimation value based on the laser radar is selected;

[0055] When the comprehensive evaluation result of the image data quality is greater than a preset threshold, the pose estimation value based on the visual sensor is selected;

[0056] Otherwise, select IMU integral pose increment;

[0057] Backend optimization subsystem, which includes:

[0058] Pose optimization module: takes the dynamically selected pose estimation value as the initial value, searches for key frames within the preset radius to build a local map, matches and optimizes the current frame point cloud with the local map, and determines whether to set the current frame as a key frame based on the pose change;

[0059] Global optimization module: Based on the pre-integration results of IMU data, loop detection results, and the pose estimation values ​​based on lidar and vision sensors, a factor graph is constructed and a nonlinear optimization algorithm is used for global optimization to obtain globally consistent pose estimation results.

[0060] Beneficial effects of the present invention:

[0061] The present invention realizes a comprehensive evaluation of the sensor data quality by comprehensively considering multiple dimensions such as the scene degradation degree, field of view completeness and intensity information entropy of point cloud data, as well as the clarity, self-similarity and natural quality of image data, and overcomes the limitation of the prior art that only relies on a single indicator for evaluation. It can effectively identify low-quality data affected by factors such as illumination changes, occlusion, motion blur, etc., and avoid the influence of low-quality data on positioning results by dynamically selecting high-quality sensor data or IMU integration results as the initial value of pose estimation, thereby significantly improving the positioning accuracy of the SLAM system in complex environments, avoiding system crashes caused by failure of a single sensor, and significantly enhancing the robustness of the SLAM system in complex environments. At the same time, when optimizing the matching of point clouds and maps, the present invention adopts a local map construction strategy and only considers key frames within a preset radius, thereby effectively reducing the computational complexity and improving the real-time performance of the SLAM system. BRIEF DESCRIPTION OF THE DRAWINGS

[0062] Figure 1 is a system architecture diagram of the present invention;

[0063] Figure 2 This is a schematic diagram of the image clarity evaluation processing window of the present invention;

[0064] Figure 3 Schematic diagram of point cloud normal vector in different scenes;

[0065] Figure 4 It is a schematic diagram of the clarity evaluation result of the image data quality evaluation index;

[0066] Figure 5 It is a schematic diagram of the self-similarity evaluation results of the image data quality assessment indicators;

[0067] Figure 6 It is a schematic diagram of the natural quality evaluation results of the image data quality assessment indicators;

[0068] Figure 7 It is a schematic diagram of the scene degradation evaluation results of the point cloud data quality evaluation index;

[0069] Figure 8 It is a schematic diagram of the field of view completeness evaluation result of the point cloud data quality assessment index;

[0070] Fig. 9 Schematic diagram of the intensity information entropy evaluation results of point cloud data quality assessment indicators. DETAILED DESCRIPTION

[0071] In order to make the purpose, technical solutions and advantages of the present invention clearer and more understandable, the present invention is further described in detail below in conjunction with the accompanying drawings and technical solutions.

[0072] At present, mainstream multi-sensor fusion SLAM algorithms such as LVI-SAM and FAST-LIVO usually use the pose estimation results of the visual subsystem as the initial value of the laser Scan to Map, or filter them with the pose estimation results of the laser subsystem to obtain the optimal pose estimation. When the sensor data quality significantly decreases in scenes with visual degradation or laser degradation, using the pose estimation results of the visual subsystem as the initial value of the laser Scan to Map as a single fusion method will lead to reduced system accuracy or even failure.

[0073] This embodiment is based on how to comprehensively evaluate the data quality of each sensor in real time, dynamically fuse the visual and laser subsystems, so as to perform more accurate pose estimation and map construction, and provides a multi-sensor fusion SLAM method based on comprehensive evaluation of data quality. The specific implementation method is as follows:

[0074] S1. Obtaining laser radar point cloud data, visual sensor image data and inertial measurement unit IMU data; where:

[0075] LiDAR point cloud data: collected in real time by the LiDAR sensor, containing 3D geometric information of the environment.

[0076] Visual sensor image data: collected in real time by the visual sensor, containing two-dimensional visual information in the environment.

[0077] Inertial Measurement Unit (IMU) data: collected in real time by the IMU sensor, including the acceleration and angular velocity information of the device, which is used to assist in posture estimation.

[0078] S2, pre-integrate the IMU data to obtain the IMU integral pose increment. At the same time, based on the point cloud data and image data, estimate the current pose of the lidar subsystem and the visual subsystem respectively. The pre-integration process takes into account the deviation and noise of the IMU, and calculates the relative pose transformation of the IMU between two consecutive time points through the numerical integration method.

[0079] Among them, the process of obtaining the IMU integral posture increment includes the following steps: first, preprocess the IMU raw data, including denoising, zero bias compensation and time synchronization; then calculate the relative posture change between two moments by the pre-integration method, and specifically use the numerical integration method (such as the median method) to integrate the angular velocity and acceleration of the IMU to obtain the increments of position, velocity and attitude; at the same time, consider the influence of gravity and covariance propagation to compensate for the error term; finally, output the IMU integral posture increment.

[0080] When calculating the pose estimate based on the LiDAR sensor, the original point cloud data is first arranged according to the LiDAR scan line number, and then the corner point and surface point feature detection are performed to output the corner point cloud and surface point cloud. Corner point features represent sharp edges in the point cloud, and surface point features represent smooth surfaces. Next, based on these feature point clouds, the pose transformation between the current frame and the previous frame is calculated, including rotation and translation. Point cloud registration is achieved through the iterative closest point (ICP) algorithm or the NDT (Normal Distributions Transform) algorithm to obtain the pose estimate based on the LiDAR sensor.

[0081] When calculating the pose estimation value based on the visual sensor, the original image data is feature extracted. Algorithms such as ORB (Oriented FAST and Rotated BRIEF) or SIFT (Scale-Invariant Feature Transform) can be used to extract feature key points and feature descriptor information. Then, the features between the current image frame and the previous frame are matched, and the epipolar constraint problem is constructed and solved based on the matching feature point pairs to initialize the three-dimensional feature map. At the same time, combined with IMU data, the visual-inertial joint maximum a posteriori estimation problem is constructed and solved, the IMU is initialized, and the motion scale is restored; after completing the map and IMU initialization, the PnP (Perspective-n-Point) algorithm can be used to solve the pose transformation between the current image frame and the previous frame; finally, the sliding window method is used to construct a local BA (Bundle Adjustment) optimization problem, and the poses of all frames in the local window are jointly optimized with the map points to obtain the pose of the visual sensor.

[0082] S2. Perform quality assessment on point cloud data and image data based on multi-dimensional quality assessment indicators;

[0083] There are many factors that affect the quality of LiDAR point cloud data, such as sensor occlusion, rain, snow and other adverse weather environments. This embodiment is based on the factors that affect the quality of LiDAR point cloud data. From the perspective of laser SLAM construction, this embodiment considers the actual processing flow of point cloud registration and uses quality evaluation indicators such as point cloud scene degradation degree, point cloud field of view completeness, and point cloud intensity information entropy to perform a multi-dimensional evaluation of point cloud data quality.

[0084] The degree of scene degradation of the point cloud is used to evaluate whether the geometric features of the point cloud are rich, so as to estimate the risk of degradation of the current scene. The richer the geometric features contained in the point cloud, the more reliable the point cloud registration result will be. Intuitively, if the normal vectors of a large number of points in the point cloud are parallel or approximately parallel, it can be determined that the geometric features of the point cloud are not obvious, such as Figure 3 The evaluation process of the degradation degree of the point cloud scene using this feature is as follows:

[0085] For each point in the point cloud , you can find the 5 nearest neighbor points and calculate the points in turn The normal vector The dot product of the normal vectors of these five points is used to determine whether the normal vectors are parallel. The judgment formula is:

[0086]

[0087] In the formula, For point The normal vectors of the 5 nearest neighbors of , , are the components of the normal vector in the x, y, and z directions.

[0088] If point The angle between the normal vector of the point and the normal vector of one of the five nearest neighboring points is greater than , then it is determined is the effective point.

[0089] The degree of point cloud scene degradation is the ratio of the number of valid points to the total number of points in the point cloud. The ratio calculation formula is: , where is the total number of points in the point cloud, is the number of valid points.

[0090] The point cloud field of view completeness is used to evaluate the field of view completeness of the point cloud actually obtained by the LiDAR sensor. Due to different installation angles of the LiDAR sensor and sensor contamination, the actual point cloud field of view size deviates from the sensor theoretical field of view size. A smaller field of view will result in lower point cloud registration accuracy and even cause the entire system to fail to operate normally. Therefore, it is necessary to consider the impact of the point cloud field of view completeness on data quality. The evaluation and processing process of the point cloud field of view completeness is as follows:

[0091] The horizontal field of view completeness is the ratio of the actual horizontal field of view to the theoretical horizontal field of view of the sensor design. The calculation formula is: ;

[0092] The vertical field of view completeness is the ratio of the actual vertical field of view to the theoretical vertical field of view of the sensor design. The calculation formula is: ;

[0093] The point cloud field of view completeness is the minimum value of the horizontal field of view completeness and the vertical field of view completeness. The calculation formula is:

[0094] .

[0095] The point cloud intensity information entropy starts from the degree of disorder of the intensity distribution of the point cloud, and indirectly reflects the complexity of the current scene. The larger the point cloud intensity information entropy, the more complex the types of objects and the distances of the objects in the scene. It can further explain that the current environment has richer geometric features. Richer geometric features are conducive to point cloud registration, and the higher the confidence of the pose estimation result.

[0096] The calculation method of point cloud intensity information entropy is: count the intensity values ​​of all points in the point cloud, find the intensity distribution cluster area, and obtain the maximum intensity value of the cluster area. , minimum strength , according to the intensity level , divided into intensity ranges;

[0097] By formula Calculate the probability of each level strength value appearing ,in, is the number of laser point clouds with intensity value of level i, and n is the total number of points in the laser point cloud;

[0098] By formula Calculate the average information entropy of point cloud intensity ,in, is the probability of occurrence of the i-th level intensity value, is the total level of intensity value;

[0099] Average information entropy of point cloud intensity After normalization, the point cloud intensity information entropy evaluation result can be obtained. The normalization formula is:

[0100]

[0101] in, is the validity threshold of intensity information entropy.

[0102] The reasons that affect the quality of image data include: lens dirt and occlusion, bad weather environment, internal sensor aging, etc. From different perspectives, there are various quality evaluation indicators for the quality of an image data. Therefore, from the perspective of visual SLAM construction, considering the actual process of using images for pose estimation, this embodiment uses evaluation methods such as image clarity, image self-similarity, and image natural quality to evaluate the image data quality in multiple dimensions.

[0103] Image clarity evaluation is an intuitive quality evaluation method. Rain, snow, fog, dirty lenses, and changes in lighting conditions can affect image clarity. In the same scene, the clearer the image, the more obvious the changes in light and dark, and the more information it contains. The clearer the image, the higher the accuracy of feature extraction and feature matching, and the more reliable the pose estimation results.

[0104] The image clarity is calculated as follows: the clearer the image, the more obvious the changes in brightness. A processing window of size 3 is designed to detect the changes in brightness of the image. For each point in the image, the current pixel point The processing window is divided into two areas according to the four directions of 0 degrees, 45 degrees, 90 degrees, and 135 degrees. The contrast in the four directions is calculated respectively, and the calculation formula is:

[0105]

[0106] in, , Represents the grayscale mean of two regions in a certain direction. Indicates the pixel point in a certain direction The maximum value of the contrast in the four directions is taken as the current pixel point The contrast ratio is calculated as: ;

[0107] like Value greater than flat area threshold , then the point is judged to be a non-flat point. Value greater than high contrast threshold , then the point is determined to be a high contrast point. Finally, using the formula Calculate the image clarity value,

[0108] in, represents the number of non-flat points, Indicates the number of high contrast points.

[0109] Image self-similarity is a quality assessment indicator for determining whether the surrounding texture information is similar. If there are a large number of similar textures in the surrounding environment, the probability of error in feature matching will increase, thereby affecting the accuracy of the pose estimation results and even causing the entire system to fail to operate normally.

[0110] The method for calculating the similarity of an image is to divide the image into tiles of the same size and calculate the similarity of each tile. Use the Perceptual Hash Algorithm (pHash) to calculate its hash value , and calculate the Hamming distance (HammingDistance) between tiles in turn .

[0111] If the tile and Hamming distance Less than the similarity threshold , then determine and are similar; if the tiles If the tile is similar to the remaining 20% ​​of tiles, then the tile is determined to be a similar tile.

[0112] The image similarity is the ratio of the number of similar tiles to the total number of tiles, and the calculation formula is:

[0113]

[0114] in, is the total number of tiles, is the number of similar tiles.

[0115] The image natural quality evaluation method is used to evaluate the impact of the image during transmission, compression, etc. This embodiment uses the BRISQUE algorithm, a reference-free spatial threshold image evaluation algorithm, as the basis for image natural quality evaluation;

[0116] The core principle of the BRISQUE algorithm is to extract mean subtracted contrastnormalized (MSCN) coefficients from the image. These coefficients reflect the local contrast normalization characteristics of the image. Subsequently, the algorithm fits these MSCN coefficients into an asymmetric generalized Gaussian distribution (AGGD), extracts the features of the fitted Gaussian distribution, and inputs them into a support vector machine (SVM) for regression to obtain the image quality evaluation results.

[0117] By normalizing the results of the BRISQUE algorithm, the natural quality of the image can be obtained. The calculation formula is: ,in, Indicates the calculation result of BRISQUE algorithm.

[0118] S3. Use a weight calculation method based on the fusion of subjective weight and objective weight to determine the weight of each quality assessment indicator and calculate the comprehensive quality score of point cloud data and image data;

[0119] Specifically, it includes: according to the sample sampling duration and sample sampling quantity, the point cloud data quality assessment results and the image data quality assessment results are evenly sampled within the sampling time range to obtain the point cloud data quality assessment sample list and the image data quality assessment sample list respectively; the subjective weight of each quality assessment indicator is determined by the G1 method, wherein the image data quality assessment indicators are sorted in the order of importance of image clarity, image self-similarity, and image natural quality, and the point cloud data quality assessment indicators are sorted in the order of importance of the point cloud scene degradation degree, point cloud field of view completeness, and point cloud intensity information entropy; the entropy weight method is used to standardize the sample data and calculate the information entropy to determine the objective weight of each quality assessment indicator; the moment estimation theory is used to determine the ratio of the subjective weight coefficient to the objective weight coefficient, and the subjective weight and the objective weight are weightedly fused to obtain the comprehensive weight.

[0120] The subjective weight is calculated by subjectively determining the relative importance of image data quality assessment indicators, such as image clarity ( )>Image similarity ( ) > Image Natural Quality ( ); Subjectively determine the relative importance of point cloud data quality assessment indicators, such as the degree of point cloud scene degradation ( )>Point cloud field of view completeness ( )>Point cloud intensity information entropy ( ); determine the ratio of the importance of each indicator, and calculate the subjective weight of each data quality assessment indicator based on the determined ratio. The calculation formula is:

[0121] ,

[0122] in, Represents the number of data quality assessment indicators, represents the subjective weight of the data quality assessment indicator, It represents the importance ratio between data quality assessment indicators.

[0123] According to the entropy weight method, the objective weight corresponding to each data quality assessment indicator is determined. The calculation method is as follows: first, the sample value of each data quality assessment indicator is standardized. Corresponding to the above image or point cloud data quality assessment indicators, where the indicators The sample value is , the corresponding sample value after standardization is , then:

[0124]

[0125] in, Indicates The first data quality assessment indicator Sample values.

[0126] Then the information entropy of each data quality assessment index is calculated. The information entropy of a data quality assessment indicator is calculated as follows:

[0127]

[0128] in, represents the number of samples, Indicates The first data quality assessment indicator After standardization, Indicates The information entropy of a data quality assessment indicator.

[0129] Finally, according to the information entropy of each indicator, the objective weight of each data quality assessment indicator is calculated. The calculation formula is:

[0130]

[0131] in, Represents the number of data quality assessment indicators, represents the objective weight of the data quality assessment indicator, Indicates The information entropy of a data quality assessment indicator.

[0132] The calculation method of comprehensive weight is:

[0133] Assume that the subjective weight importance is expressed as , the relative importance of objective weights is expressed as According to the basic theoretical idea of ​​moment estimation method, the relative importance coefficient of subjective and objective weights of each indicator is calculated. The calculation formula is:

[0134]

[0135] in, Indicates The subjective weight and objective weight of each data quality assessment indicator , Indicates The subjective weight importance coefficient and objective weight importance coefficient of each data quality assessment indicator;

[0136] Finally, according to the importance coefficients of subjective and objective weights, the subjective weights and objective weights are weighted and fused to obtain the comprehensive weights of the corresponding indicators. The fusion formula is:

[0137]

[0138] S4. According to the comprehensive quality scores of the point cloud data and the image data, dynamically select the pose estimation value based on the lidar, the pose estimation value based on the visual sensor, or the IMU integrated pose increment as the optimization initial value; the method for dynamically selecting the optimization initial value is:

[0139] When the comprehensive evaluation result of the point cloud data quality is greater than the preset threshold, the pose estimation value based on the lidar sensor is selected as the optimization initial value;

[0140] When the comprehensive evaluation result of the image data quality is greater than the preset threshold, the pose estimation value based on the visual sensor is selected as the optimization initial value;

[0141] Otherwise, the IMU integral pose increment is selected as the initial value for optimization.

[0142] S5. Based on the selected initial value, the matching optimization between the point cloud and the map and the global pose graph optimization are performed to obtain the optimal pose estimation value and point cloud map.

[0143] Specifically, ScantoMap pose optimization can be used, including:

[0144] Based on the dynamically selected pose estimation value, key frames are found within the preset radius to build a local map;

[0145] Match and optimize the current frame point cloud with the local map, and use the ICP algorithm or NDT algorithm to achieve point cloud registration;

[0146] Whether to set the current frame as a key frame is determined based on the amount of posture change. If the amount of posture change is greater than a preset threshold (such as 0.5 meters or 5 degrees), the current frame is set as a key frame.

[0147] Global pose optimization includes:

[0148] Perform pre-integration calculation based on IMU data;

[0149] Use loop detection technology such as the bag-of-words model to detect the loop relationship between the current frame and historical key frames;

[0150] The key frame pose, IMU pre-integration value, and loop detection results are used as prior and constraint factors to construct a factor graph optimization problem.

[0151] A nonlinear optimization algorithm is used to optimize the factor graph to obtain a globally consistent pose estimation result; the nonlinear optimization algorithm may be a Levenberg-Marquardt algorithm or a Gauss-Newton algorithm.

[0152] like Figure 1 The embodiment of the present invention provides a multi-sensor fusion SLAM system based on comprehensive data quality evaluation to implement the above method, the system comprising:

[0153] IMU pre-integration module: receives IMU data, performs pre-integration based on IMU data, calculates IMU integral posture increment, and outputs it to the data quality comprehensive evaluation module;

[0154] LiDAR odometer subsystem, which includes:

[0155] Point cloud preprocessing module: used to receive point cloud data collected by the laser radar in real time, perform corner point and surface point feature detection, and output corner point and surface point feature point cloud;

[0156] Inter-frame odometry module: used to receive feature point clouds, calculate the pose transformation between the current point cloud frame and the previous frame based on corner point and surface point features, output the pose estimation value based on the lidar, and pass the estimated pose information to the data quality comprehensive evaluation module;

[0157] Visual odometry subsystem, which includes:

[0158] Feature extraction and matching module: used to receive image data collected by the visual sensor in real time, extract feature key points and feature descriptor information, and perform feature matching between image frames;

[0159] Image frame pose estimation module: used to construct and solve the epipolar constraint problem based on matching feature point pairs, initialize the three-dimensional feature map, calculate the pose transformation between image frames using the pose solving algorithm, and pass the estimated pose information to the local BA optimization module;

[0160] Local BA optimization module: The sliding window method is used to construct the local BA optimization problem, the poses and map points of all frames in the local window are jointly optimized, and the optimized pose estimation values ​​based on the visual sensor are passed to the comprehensive data quality evaluation module;

[0161] The data quality multi-index comprehensive evaluation subsystem includes:

[0162] Point cloud data quality assessment module: multi-dimensional quality assessment of point cloud data based on scene degradation, field of view integrity and intensity information entropy;

[0163] Image data quality assessment module: performs multi-dimensional quality assessment of image data based on image clarity, self-similarity and natural quality;

[0164] Data quality comprehensive evaluation module: Dynamically select the pose estimation value based on lidar, the pose estimation value based on visual sensor or the IMU integral pose increment as the optimization initial value, where:

[0165] When the comprehensive evaluation result of the point cloud data quality is greater than the preset threshold, the pose estimation value based on the laser radar is selected;

[0166] When the comprehensive evaluation result of the image data quality is greater than a preset threshold, the pose estimation value based on the visual sensor is selected;

[0167] Otherwise, select IMU integral pose increment;

[0168] Backend optimization subsystem, which includes:

[0169] Pose optimization module: takes the dynamically selected pose estimation value as the initial value, searches for key frames within the preset radius to build a local map, matches and optimizes the current frame point cloud with the local map, and determines whether to set the current frame as a key frame based on the pose change;

[0170] Global optimization module: Based on the pre-integration results of IMU data, loop detection results, and the pose estimation values ​​based on lidar and vision sensors, a factor graph is constructed and a nonlinear optimization algorithm is used for global optimization to obtain globally consistent pose estimation results.

[0171] In this specification, each embodiment is described in a progressive manner, and each embodiment focuses on the differences from other embodiments. The same or similar parts between the embodiments can be referred to each other. For the system disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant parts can be referred to the method part.

[0172] Unless otherwise specifically stated, the relative steps, numerical expressions and numerical values ​​of the components and steps set forth in the above-described embodiments do not limit the scope of the present invention.

[0173] The units and method steps of each example described in conjunction with the embodiments disclosed herein can be implemented by electronic hardware, computer software, or a combination of the two. In order to clearly illustrate the interchangeability of hardware and software, the composition and steps of each example have been generally described in the above description according to function. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. A person of ordinary skill in the art may use different methods to implement the described functions for each specific application, but such implementation is not considered to be beyond the scope of the present invention.

Claims

1. A multi-sensor fusion SLAM method based on comprehensive data quality evaluation, characterized in that: include: Acquire LiDAR point cloud data, visual sensor image data, and inertial measurement unit (IMU) data; Perform pre-integration processing on the IMU data to obtain the IMU integral pose increment, and calculate the pose estimation value based on the lidar and the pose estimation value based on the visual sensor at the current moment based on the point cloud data and image data respectively; The quality of point cloud data and image data is evaluated based on multi-dimensional quality evaluation indicators, including: Point cloud data quality assessment indicators include scene degradation, field of view completeness, and intensity information entropy; Image data quality assessment indicators include image clarity, self-similarity, and natural quality; A weight calculation method based on the fusion of subjective weight and objective weight is used to determine the weight and comprehensive weight of each quality assessment indicator, and calculate the comprehensive quality score of point cloud data and image data; According to the comprehensive quality scores of point cloud data and image data, dynamically select the pose estimation value based on lidar, the pose estimation value based on visual sensor or the IMU integrated pose increment as the optimization initial value; Based on the selected optimization initial value, the matching optimization between the point cloud and the map and the global pose graph optimization are performed to obtain the optimal pose estimation value and point cloud map.

2. The multi-sensor fusion SLAM method based on comprehensive data quality evaluation according to claim 1, characterized in that: The steps of calculating the laser radar-based pose estimation value include: arranging the original point cloud data according to the laser radar scanning line number, performing corner point and surface point feature detection, outputting corner point cloud and surface point point cloud, calculating the pose transformation between the current frame and the previous frame based on the corner point cloud and the surface point cloud, and obtaining the laser radar-based pose estimation value.

3. The multi-sensor fusion SLAM method based on comprehensive data quality evaluation according to claim 1, characterized in that: The steps of calculating the pose estimation value based on the visual sensor include: extracting features from the original image data to obtain feature key points and feature descriptor information, matching the features between the current image frame and the previous frame; constructing and solving the epipolar constraint problem based on the matching feature point pairs, and initializing the three-dimensional feature map; calculating the pose transformation between the current image frame and the previous frame using the pose solving algorithm; constructing a local BA optimization problem using the sliding window method, jointly optimizing the poses of all frames in the local window with the map points, and obtaining the pose estimation value based on the visual sensor.

4. The multi-sensor fusion SLAM method based on comprehensive data quality evaluation according to claim 1, characterized in that: The calculation method of the point cloud data quality evaluation index includes: The degree of scene degradation is to calculate the angle between the normal vectors of each point in the point cloud and its K nearest neighboring points. If there is an angle greater than the preset angle threshold, it is determined to be a valid point. The ratio of the number of valid points to the total number of points in the point cloud is calculated to obtain the degree of scene degradation. Field of view completeness: calculate the ratio of the actual horizontal field of view to the theoretical horizontal field of view of the LiDAR sensor design to get the horizontal field of view completeness, and calculate the ratio of the actual vertical field of view to the theoretical vertical field of view of the LiDAR sensor design to get the vertical field of view completeness; take the minimum value of the horizontal field of view completeness and the vertical field of view completeness as the point cloud field of view completeness; Intensity information entropy, the intensity values ​​of all points in the point cloud are counted, the probability distribution entropy of the intensity distribution is calculated, and the entropy value is normalized to obtain the point cloud intensity information entropy.

5. The multi-sensor fusion SLAM method based on comprehensive data quality evaluation according to claim 1, characterized in that: The calculation method of the image data quality assessment index includes: Image clarity: for each pixel in the image, a processing area with a preset window size is designed with the current pixel as the center, and the local contrast is calculated; the number of non-flat points and high-contrast points is counted, and the weighted sum is calculated as the image clarity; Self-similarity: divide the image into tiles of the same size, use the perceptual hashing algorithm to calculate the similarity between tiles, count the proportion of similar tiles, and obtain the image self-similarity; Natural quality,extracts characteristic coefficients from the image, fits them into statistical distribution features,and regresses them through a machine learning model to obtain the natural quality of the image.

6. The multi-sensor fusion SLAM method based on comprehensive data quality evaluation according to claim 1, characterized in that: The calculation method of the weight and comprehensive weight of each quality assessment indicator includes: According to the preset sample sampling duration and sample sampling quantity, the point cloud data quality assessment results and the image data quality assessment results are evenly sampled within the sampling time range to obtain the point cloud data quality assessment sample list and the image data quality assessment sample list respectively; the subjective ranking method is used to determine the subjective weight of each quality assessment indicator; the entropy weight method is used to standardize the sample data and calculate the information entropy to determine the objective weight of each quality assessment indicator; the weighted fusion method is used to comprehensively calculate the subjective weight and the objective weight to obtain the comprehensive weight.

7. The multi-sensor fusion SLAM method based on comprehensive data quality evaluation according to claim 1, characterized in that: The step of optimizing the matching between the point cloud and the map includes: Based on the dynamically selected pose estimation value, key frames are found within the preset radius to build a local map; Match the current frame point cloud with the local map and optimize the pose; Determine whether to set the current frame as a key frame based on the change in posture.

8. The multi-sensor fusion SLAM method based on comprehensive data quality evaluation according to claim 1, characterized in that: The steps of global pose graph optimization include: Perform pre-integration calculation based on IMU data; Use loop detection technology to detect the loop relationship between the current frame and historical key frames; The key frame pose, IMU pre-integration value, and loop detection result are used as priors and constraint factors to construct a factor graph optimization problem. A nonlinear optimization algorithm is used to optimize the factor graph to obtain globally consistent pose estimation results.

9. The multi-sensor fusion SLAM method based on comprehensive data quality evaluation according to claim 1, characterized in that: Methods for dynamically selecting optimization initial values ​​include: When the comprehensive evaluation result of the point cloud data quality is greater than the preset threshold, the pose estimation value based on the lidar sensor is selected as the optimization initial value; When the comprehensive evaluation result of the image data quality is greater than the preset threshold, the pose estimation value based on the visual sensor is selected as the optimization initial value; Otherwise, the IMU integral pose increment is selected as the initial value for optimization.

10. A multi-sensor fusion SLAM system based on comprehensive data quality evaluation, characterized in that: Used to implement the method according to any one of claims 1 to 9, comprising: IMU pre-integration module: receives IMU data, performs pre-integration based on IMU data, calculates IMU integral posture increment, and outputs it to the data quality comprehensive evaluation module; LiDAR odometer subsystem, which includes: Point cloud preprocessing module: used to receive point cloud data collected by the laser radar in real time, perform corner point and surface point feature detection, and output corner point and surface point feature point cloud; Inter-frame odometry module: used to receive feature point clouds, calculate the pose transformation between the current point cloud frame and the previous frame based on corner point and surface point features, output the pose estimation value based on the lidar, and pass the estimated pose information to the data quality comprehensive evaluation module; Visual odometry subsystem, which includes: Feature extraction and matching module: used to receive image data collected by the visual sensor in real time, extract feature key points and feature descriptor information, and perform feature matching between image frames; Image frame pose estimation module: used to construct and solve the epipolar constraint problem based on matching feature point pairs, initialize the three-dimensional feature map, calculate the pose transformation between image frames using the pose solving algorithm, and pass the estimated pose information to the local BA optimization module; Local BA optimization module: The sliding window method is used to construct the local BA optimization problem, the poses and map points of all frames in the local window are jointly optimized, and the optimized pose estimation values ​​based on the visual sensor are passed to the comprehensive data quality evaluation module; The data quality multi-index comprehensive evaluation subsystem includes: Point cloud data quality assessment module: multi-dimensional quality assessment of point cloud data based on scene degradation, field of view integrity and intensity information entropy; Image data quality assessment module: performs multi-dimensional quality assessment of image data based on image clarity, self-similarity and natural quality; Data quality comprehensive evaluation module: Dynamically select the pose estimation value based on lidar, the pose estimation value based on visual sensor or the IMU integral pose increment as the optimization initial value, where: When the comprehensive evaluation result of the point cloud data quality is greater than the preset threshold, the pose estimation value based on the laser radar is selected; When the comprehensive evaluation result of the image data quality is greater than a preset threshold, the pose estimation value based on the visual sensor is selected; Otherwise, select IMU integral pose increment; Backend optimization subsystem, which includes: Pose optimization module: takes the dynamically selected pose estimation value as the initial value, searches for key frames within the preset radius to build a local map, matches and optimizes the current frame point cloud with the local map, and determines whether to set the current frame as a key frame based on the pose change; Global optimization module: Based on the pre-integration results of IMU data, loop detection results, and the pose estimation values ​​based on lidar and vision sensors, a factor graph is constructed and a nonlinear optimization algorithm is used for global optimization to obtain globally consistent pose estimation results.

Images