SLAM method and system with self-diagnosis function

Abstract

The invention discloses an SLAM (Simultaneous Localization and Mapping) method and system with a self-diagnosis function, and the method comprises the steps: receiving and analyzing the credibility and integrity of sensor data, and outputting the health score of each sensor and the overall health score of the SLAM system; checking the space-time consistency between the sensor data, judging whether a sensor fault exists or not, and outputting a detection result; classifying sensor faults according to the detection result, and taking corresponding isolation measures; the fusion weight of the sensor is dynamically adjusted, and the influence of the fault sensor and the normal working sensor on the SLAM system is adjusted; when the sensor breaks down, an alarm is given out, and meanwhile fault information is recorded. According to the method, the states of various sensors can be monitored in real time, self-diagnosis is carried out, a dynamic self-adaptive data fusion strategy is executed, a multi-sensor fusion algorithm is adjusted in real time according to the health states of the sensors, and normal operation of the system under the condition that part of the sensors fail is guaranteed.

Description

Technical Field

[0001] This invention belongs to the field of SLAM (Simultaneous Localization and Mapping) technology, specifically relating to a SLAM method and system with self-diagnostic function. Background Technology

[0002] In industrial environments, robots are widely used in logistics, manufacturing, and inspection. SLAM technology, as the core of autonomous robot navigation, enables robots to locate themselves and build environmental maps in unknown environments. However, industrial environments often have harsh conditions such as high temperature, high humidity, strong electromagnetic interference, and dust, which can easily lead to sensor failures, such as contaminated LiDAR, blurred cameras, and IMU data drift.

[0003] Traditional SLAM systems can lead to localization errors and map building failures, potentially causing safety incidents. Traditional SLAM system architectures lack dynamic assessment mechanisms for sensor health. When a single sensor exhibits abnormal output due to environmental interference, the system cannot effectively identify changes in data reliability and continues to process degraded signals. This results in accumulated errors in pose estimation, causing inaccurate map building or navigation path deviations, ultimately leading to robot collisions with obstacles, entry into hazardous areas, or disruptions to workflows, severely threatening production continuity and equipment safety in industrial settings. Summary of the Invention

[0004] Technical Objective: To address the aforementioned technical problems, this invention proposes a SLAM method and system with self-diagnostic capabilities, which can monitor the health status of various sensors in real time, promptly detect data reliability issues, thereby reducing positioning errors and improving the reliability and security of the SLAM system.

[0005] Technical solution: To achieve the above technical objectives, the present invention adopts the following technical solution:

[0006] A SLAM method with self-diagnostic capabilities includes the following steps:

[0007] S1. Real-time monitoring of various connected sensors, including LiDAR, camera, IMU and visual odometry, receiving and analyzing the reliability and integrity of sensor data, and outputting the health score of each sensor and the overall health score of the SLAM system.

[0008] S2. Check the spatiotemporal consistency between sensor data, determine if there is a sensor fault, and output the detection results;

[0009] S3. Based on the detection results, classify the sensor faults into transient faults, continuous faults, and progressive faults, and take corresponding isolation measures.

[0010] S4. Dynamically adjust sensor fusion weights to adjust the impact of faulty and normal sensors on the SLAM system;

[0011] S5. When a sensor malfunctions, an alarm is triggered and the fault information is recorded.

[0012] Preferably, step S1 executes the lidar health detection process, specifically including the following steps:

[0013] Scan frequency detection: Read the current rotational speed from the radar status message. Compared with nominal value Comparison, if This was determined to be an abnormal frequency.

[0014] Point cloud density statistics: Count the number of points in each frame of the point cloud. The theoretical expected number of points for the distance range estimation from the environment. Calculate the density ratio If M consecutive frames satisfy The system determines that there is obstruction or contamination.

[0015] Distance value distribution and noise level: Statistical analysis of distance values ​​collected from multiple frames within the same angular range, calculating the mean and standard deviation. with standard deviation ,like If the value is 3 times or more than the historical average, it is judged as noise anomaly or optical interference;

[0016] Health score calculation: Set frequency score Density score Noise score By weight , , Perform a weighted summation, that is .

[0017] Preferably, step S1 performs a camera health check process, which specifically includes the following steps:

[0018] Brightness and contrast check: Calculate the mean of the grayscale image. Standard deviation ,like or Determine if the brightness is abnormal; If the value is close to 0, it is determined to be lens occlusion or strong blur.

[0019] Image sharpness detection: Calculating image edge response energy using the Laplacian operator: Where N is the number of pixels. It is a pixel The Laplace response, if Below the threshold And this continued for several frames, which was determined to be lens contamination or poor focus;

[0020] Feature point extraction success rate: Feature points were extracted using ORB and SIFT algorithms, and the number of feature points was counted. ,like If the value is 30% lower than the average value during the normal period, it is considered that the data quality has declined.

[0021] Health score calculation: Set brightness score Clarity rating Feature point scoring The overall score is obtained by weighting and summing the results according to their respective weights. .

[0022] Preferably, step S1 executes the IMU health check process, which specifically includes the following steps:

[0023] Output frequency and heartbeat detection: Check whether the time interval between adjacent data packets is within the allowable range. If it is not within the allowable range, it is considered a communication abnormality.

[0024] Noise variance and bias drift monitoring: By estimating motion results using wheel speed odometers and lidar point clouds, the system determines whether the vehicle is stationary. Through a 1-second sliding window, the mean and standard deviation of acceleration and angular velocity for each axis are calculated. If the standard deviation exceeds the calibration value by 2-3 times and remains so for T1, the noise is considered excessive. If the mean angular velocity deviates from 0 or the mean acceleration deviates too much from gravitational acceleration when stationary, bias drift is identified.

[0025] Health score calculation: The health score of the IMU is calculated based on the results of noise variance and bias drift assessment. .

[0026] Preferably, the multi-sensor health monitoring module performs a visual odometry health check process, specifically including the following steps:

[0027] Monitor the matching degree between the odometer pulse frequency and the robot's actual speed. If the deviation exceeds 20%, it is judged as mechanical wear or slippage failure.

[0028] Health score calculation: Calculate the health score of the odometer based on its deviation. .

[0029] Preferably, step S1 executes the overall health scoring process of the system:

[0030] By lidar weight ,camera IMU weights Odometer weighting Weighted by 0.15, the overall system health score is generated. + + .

[0031] Preferably, step S2 specifically includes the following steps:

[0032] S2.1 Detecting the consistency of visual features between LiDAR and map: Using the current map to predict the occupancy probability distribution obtained by laser scanning, match it with the actual point cloud, calculate the matching cost, and determine whether there is an anomaly in the LiDAR or map based on the calculation result of the matching cost and the change of the LiDAR health score.

[0033] S2.2 Detecting the consistency between the IMU and the visual odometry: Obtaining the pose increment from the visual odometry. The pose increment is obtained by IMU integration. Calculate the difference vector And calculate the norm or Mahalanobis distance, if K consecutive frames Exceeding the threshold If so, it is considered that there is a consistency problem between the two;

[0034] S2.3 Model Prediction Detection: Using kinematic models or environmental priors, the sensor output is predicted and compared with the actual observation. If the predicted value exceeds the actual observation value, an alarm is triggered.

[0035] S2.4 Statistical Methods Detection: Apply statistical methods to monitor changes in the statistical characteristics of sensor data, and perform estimation and residual analysis on the sensor data.

[0036] Preferably, step S4 specifically includes the following steps:

[0037] S4.1 Constructing the weight mapping function:

[0038] For N sensors participating in the fusion, the initial nominal weights are: Real-time weights :

[0039]

[0040] in, Assess the health of sensor i. Weight mapping function:

[0041]

[0042] In the formula, The failure threshold For health threshold, This is the adjustment coefficient;

[0043] S4.2, Application of the Fusion Framework:

[0044] Extended Kalman filtering or error-state Kalman filtering will The reciprocal relationship is reflected in the observation noise covariance matrix. middle, and Proportional to this, the observation noise of sensors with low weight is set to exceed a preset value;

[0045] Construct a graph optimization framework, and Information moments as corresponding constraints scaling factor, By controlling the weights, the influence of the corresponding constraints on the optimization weight mapping function can be adjusted.

[0046] Preferably, step S4 includes step S4.3, emergency switching mechanism:

[0047] When the health score of a single sensor Persistently below If the fault exceeds K frames, it is determined to be a persistent fault. The weights of the corresponding sensor are reset to zero, it is removed from the fusion list, and the redundancy mechanism is triggered to start a backup sensor of the same type. If there is no hardware redundancy, it switches to degrade mode and adjusts the parameters of the normally operating sensor.

[0048] A SLAM system with self-diagnostic capabilities includes:

[0049] The multi-sensor health monitoring module is used to monitor multiple connected sensors, including LiDAR, camera, IMU and odometer, in real time, receive and analyze the reliability and integrity of sensor data, and output the health scores of each sensor and the overall health score of the SLAM system.

[0050] The self-diagnosis and fault detection mechanism module is used to check the spatiotemporal consistency between sensor data, determine whether there is a sensor fault, and output the detection results.

[0051] The fault classification and isolation module is used to classify sensor faults according to the detection results, including transient faults, continuous faults, and progressive faults, and to take corresponding isolation measures.

[0052] The adaptive strategy adjustment module is used to dynamically adjust the sensor fusion weights and adjust the impact of faulty and normal sensors on the SLAM system.

[0053] The alarm and recording mechanism module is used to issue an alarm and record fault information when a sensor malfunctions.

[0054] Beneficial effects: Due to the adoption of the above technical solution, the present invention has the following beneficial effects:

[0055] (i) The SLAM method with self-diagnosis function proposed in this invention can perform self-diagnosis by monitoring the status of various sensors in real time, execute a dynamic adaptive data fusion strategy, and adjust the multi-sensor fusion algorithm in real time according to the health status of the sensors, so as to ensure the normal operation of the system in the event of failure of some sensors.

[0056] (ii) The SLAM system with self-diagnosis function proposed in this invention integrates four major modules: multi-sensor health monitoring, self-diagnosis and fault detection, fault classification and isolation, and adaptive strategy adjustment. It can realize real-time evaluation of sensor status, accurate identification and processing of faults, and dynamic adjustment of SLAM algorithm behavior to ensure that the system can maintain high reliability, robustness and safety when sensor performance is damaged or malfunctions. Attached Figure Description

[0057] Figure 1 This is an architecture diagram of a SLAM system with self-diagnostic function proposed in an embodiment of the present invention. Detailed Implementation

[0058] The embodiments of the present invention will be described in detail below.

[0059] like Figure 1 As shown, this embodiment of the invention proposes a SLAM system with self-diagnosis function, comprising:

[0060] 1. Multi-sensor health monitoring module

[0061] The multi-sensor health monitoring module is used to connect various sensors such as lidar, cameras, IMU, etc., and to monitor each connected sensor in real time, analyze the reliability and integrity of the output data of individual sensors, and output the health score of individual sensors and the overall health score of the system.

[0062] Specifically, the multi-sensor health monitoring module includes detecting the scanning frequency, point cloud density, distance value distribution, and noise level of the lidar; detecting the brightness, contrast, image clarity, and feature point extraction success rate of the camera; monitoring the output frequency, heartbeat, noise variance, and bias drift of the IMU; and monitoring the pulse frequency and speed matching degree of the odometer.

[0063] 2. Self-diagnosis and fault detection mechanism module

[0064] The self-diagnosis and fault detection mechanism module employs multiple methods to comprehensively detect abnormal sensor data and system faults, including:

[0065] Data consistency testing: Check the spatiotemporal consistency between data from different sensors, such as the consistency between LiDAR and map / visual features, and the consistency between IMU and visual odometry pose increments. Cross-validate whether the information provided by different sensors is contradictory, for example, whether the pose change calculated by visual odometry is consistent with the pose change obtained by IMU integration.

[0066] Model-based prediction detection: Predicts sensor outputs using kinematic models or environmental priors and compares them with actual observations. Based on the robot's motion model or environmental map, it predicts the data the sensors should observe and compares this prediction with actual observations; if the deviation is too large, an alarm is triggered.

[0067] Statistical methods: Methods such as statistical process control are applied to monitor changes in the statistical characteristics of sensor data. Statistical methods detect abnormal fluctuations by analyzing statistical features such as the mean and variance of sensor data.

[0068] 3. Fault Classification and Isolation Module:

[0069] The fault classification and isolation module classifies faults into types such as transient faults, persistent faults, and progressive faults based on the detection results, and takes corresponding isolation measures for different types of faults to prevent fault propagation from affecting the overall system.

[0070] 4. Adaptive Strategy Adjustment Module

[0071] The adaptive strategy adjustment module is used to adjust the data fusion strategy and SLAM algorithm parameters in real time and dynamically based on sensor health scores and fault types.

[0072] 5. Alarm and Recording Mechanism Module

[0073] The alarm and logging mechanism module is used to issue alarms and record fault information when a sensor malfunctions. Upon a fault, it promptly sends an alarm to the system or operators and records fault information for subsequent analysis and maintenance. A complete fault log includes the fault type, time, location, and relevant sensor data for later analysis and improvement.

[0074] The present invention will now describe in detail a method for a SLAM system with self-diagnostic capabilities.

[0075] I. Multi-sensor health monitoring module:

[0076] Real-time monitoring of various connected sensors such as LiDAR, cameras, and IMUs is performed, and the reliability and completeness of their output data are analyzed. Individual sensor health scores and overall system health scores are output.

[0077] S1, LiDAR Health Detection

[0078] LiDAR health detection specifically includes the following steps:

[0079] S1.1 Scanning Frequency Detection: Read the current rotational speed from the radar status message. Compared with nominal value Comparison, if (default = 1Hz), which is judged as a frequency abnormality.

[0080] S1.2 Point Cloud Density Statistics: Count the number of points in the point cloud for each frame. The theoretical expected number of points for the distance range estimation from the environment. Calculate the density ratio If M consecutive frames (default M = 5) satisfy... (default = 0.5), indicating the presence of obstruction or contamination.

[0081] S1.3 Distance Value Distribution and Noise Level: Statistical analysis of distance values ​​collected from multiple frames within the same angular range, calculating the mean and standard deviation. with standard deviation ,like If the value is 3 times or more than the historical average, it is judged as noise anomaly or optical interference.

[0082] S1.4 Health Score Calculation: Set Frequency Scoring Density score Noise score By weight =0.3, =0.4, =0.3 weighted sum, i.e. .

[0083] S2, Camera Health Check

[0084] The camera health check includes the following steps:

[0085] S2.1 Brightness and Contrast Check: Calculate the mean of the grayscale image. Standard deviation ,like (Too dark) or (Overexposure) indicates abnormal brightness; if If the value is close to 0, it is determined to be lens occlusion or strong blur.

[0086] S2.2 Image sharpness (blurriness) detection: Calculate the image edge response energy using the Laplacian operator: Where N is the number of pixels. It is a pixel The Laplace response, if Below the threshold Furthermore, if this continues for multiple frames, it is determined to be due to lens contamination or poor focus.

[0087] S2.3 Feature point extraction success rate: Feature points are extracted using the ORB / SIFT algorithm, and the number of feature points is counted. ,like If the data quality is 30% lower than the average for the normal period, it is considered a decline in data quality.

[0088] S2.4 Health Score Calculation: Set Brightness Score Clarity rating Feature point scoring The overall score is obtained by weighting and summing the results according to their respective weights. (Example weights: 0.2 / 0.4 / 0.4).

[0089] S3, IMU Health Check

[0090] S3.1 Output frequency and heartbeat detection: Check whether the time interval between adjacent data packets is within the allowable range (e.g., 1 / 100 second ± 20%), otherwise it is considered a communication abnormality.

[0091] S3.2 Noise Variance and Bias Drift Monitoring: By estimating motion results using wheel speed odometers and LiDAR point clouds, and determining whether the vehicle is stationary, the mean and standard deviation of acceleration and angular velocity for each axis are calculated using a 1-second sliding window (100 samples). If the standard deviation exceeds the calibration value by 2-3 times and persists for T1 (default T1=5s), the noise is considered too high. If the mean angular velocity deviates from 0 or the mean acceleration deviates too much from the gravitational acceleration when stationary, it is considered bias drift.

[0092] S4. Odometer Health Check:

[0093] Monitor the matching degree between the odometer pulse frequency and the robot's actual speed. If the deviation exceeds 20%, it is judged as a mechanical wear or slippage fault, and a health score is issued. It decays linearly according to the degree of deviation.

[0094] S5, Overall System Health Score:

[0095] By weight =0.4 (LiDAR) =0.2 (camera) =0.25 (IMU) =0.15 (odometer) weighted average, generating the overall system health score: + + The score is input into the self-diagnosis module and the adaptive fusion module.

[0096] II. Self-diagnosis and fault detection mechanism module

[0097] This module performs data consistency checks: it detects outliers or deviations by examining the spatiotemporal consistency between sensor data. It leverages the redundancy and complementarity between sensors, for example:

[0098] S2.1 Consistency between LiDAR and map / visual features:

[0099] The occupancy probability distribution obtained by predicting the laser scan using the current map is matched with the actual point cloud, and the matching cost (such as the mean of the ICP residual) is calculated. If the mean and variance of the residual suddenly increase and the health score decreases at the same time, it is determined that there is an anomaly in the lidar or map.

[0100] S2.2, IMU and visual odometry consistency:

[0101] Pose increment obtained from visual odometry The pose increment is obtained by integration using the IMU. Calculate the difference vector And calculate its norm or Mahalanobis distance, if there are K consecutive frames (e.g., K=10). Exceeding the threshold If so, then it is believed that there is a consistency problem between the two.

[0102] S2.3 Model Prediction Detection:

[0103] Using kinematic models or environmental priors, sensor outputs are predicted and compared with actual observations. For example, based on the current speed and direction, the distribution of laser point clouds at the next location is predicted. The location of visual features is predicted using an environmental map. The predicted values ​​are compared with the actual observed values, and an alarm is triggered if the values ​​exceed a threshold.

[0104] S2.4 Statistical Methods:

[0105] Methods such as Statistical Process Control (SPC) are applied to monitor changes in the statistical characteristics of data. For example, the mean, variance, skewness, and kurtosis of sensor data are calculated. Kalman filtering and other methods are used to estimate data and perform residual analysis.

[0106] III. Fault Classification and Isolation Module

[0107] The fault classification and isolation module executes the following process:

[0108] S3.1 Fault Classification:

[0109] Based on the test results, faults are categorized into transient faults, persistent faults, and progressive faults. Transient faults: Data anomalies occurring within a short period, such as momentary obstruction. Persistent faults: Continuous sensor failure, such as damage to a LiDAR sensor. Progressive faults: Gradual performance degradation, such as a camera lens gradually becoming blurry.

[0110] S3.2 Fault Isolation:

[0111] Locate the specific sensor or module that has malfunctioned to prevent the fault from propagating and affecting the overall system. Implement appropriate isolation measures for different types of faults; for example, automatically filter abnormal data for transient faults to avoid impacting system operation. For persistent faults, promptly isolate the faulty sensor and adjust the data fusion strategy. For progressive faults that reach the warning threshold, promptly notify the system or operators for handling.

[0112] IV. Adaptive Strategy Adjustment Module:

[0113] The adaptive strategy adjustment module of this system adjusts the data fusion strategy and SLAM algorithm parameters in real time and dynamically based on the sensor health score and fault type output by the self-diagnosis module, so as to maintain the reliability of system positioning and mapping when sensor performance degrades or fails.

[0114] The specific steps are as follows:

[0115] S4.1 Dynamic weight adjustment for sensor data fusion

[0116] Core idea: To incorporate the health score of sensors (such as LiDAR) The camera This is directly converted into its weight in multi-sensor fusion. The higher the health score, the greater the weight; when the health score is below the threshold, the weight decreases significantly or even becomes zero.

[0117] Implementation method (taking a tightly coupled LiDAR-Vision-IMU system as an example):

[0118] S4.1.1 Weight Normalization Function:

[0119] For N sensors participating in the fusion, their initial nominal weights are: (Based on preset sensor accuracy and importance, e.g., LiDAR: 0.5, Vision: 0.3, IMU: 0.2). The real-time weight W_(i, realtime) is calculated as follows:

[0120]

[0121] in Assign a health score (0-1) to sensor i. This is the weight mapping function. This function is designed to be non-linear, as follows:

[0122]

[0123] In the formula, The failure threshold (e.g., 0.2). The health threshold (e.g., 0.7). This is an adjustment factor (e.g., 5). When from Towards As the weight decreases, it decays exponentially, rapidly reducing the impact of faulty sensors.

[0124] S4.1.2, Application of the Fusion Framework:

[0125] In Extended Kalman Filtering (EKF) or Error State Kalman Filtering (ESKF): The reciprocal relationship is reflected in the observation noise covariance matrix. middle. and It is directly proportional. Sensors with lower weights have higher observation noise and lower confidence levels during filter updates.

[0126] In the graph optimization framework: Information moments as corresponding constraints (edges) The scaling factor. If the weight decreases, the influence of this constraint on the objective function will weaken.

[0127] S4.1.3 Emergency Switching Mechanism:

[0128] When a sensor's health score If the T_fail value remains below K frames (e.g., K=10), it is considered a persistent fault. The system will:

[0129] Reset the sensor weight to zero and remove it from the fusion list.

[0130] Trigger redundancy mechanisms, such as activating a backup sensor of the same type.

[0131] If there is no hardware redundancy, switch to degrade mode. For example, if the LiDAR fails, switch to pure visual odometry + IMU fusion mode and adjust the parameters of subsequent modules accordingly.

[0132] This invention utilizes a multi-sensor health monitoring module to continuously collect raw data from various sensors, including LiDAR, cameras, IMUs, and odometers. This data is then subjected to refined analysis, and a quantified health score is generated for each sensor based on a pre-defined index system. These scores intuitively reflect the current operational status and data quality of the sensors. A self-diagnosis and fault detection mechanism module performs in-depth analysis of sensor data to identify potential faults. Once an anomaly is detected, a fault classification and isolation module immediately intervenes. It accurately classifies faults based on their duration, scope of impact, and nature. Finally, an adaptive strategy adjustment module dynamically adjusts the behavior of the SLAM system based on the health scores and fault classification results.

[0133] The sensor health score designed in this invention is non-linearly mapped to real-time weights for data fusion. Sensors with high health scores receive higher weights, and their data plays a greater role in localization and mapping; while sensors with low health scores have significantly reduced weights, or even zero weights, thereby reducing their adverse impact on the system. When the health score of a sensor remains below the failure threshold, an emergency switching mechanism is activated. The system completely removes the faulty sensor and, depending on the situation, activates a backup sensor or switches to a degraded operation mode, adjusting other SLAM algorithm parameters accordingly to maintain optimal localization and mapping performance under impaired conditions. The entire process, through real-time monitoring, intelligent diagnosis, and flexible adjustments, ensures that the SLAM system maintains high reliability and safety under various complex operating conditions.

[0134] The foregoing has shown and described the basic principles, main features, and advantages of the present invention. Those skilled in the art should understand that the above embodiments do not limit the present invention in any way, and all technical solutions obtained by equivalent substitution or equivalent transformation fall within the protection scope of the present invention.

Claims

1. A SLAM method with self-diagnostic function, characterized in that, Including the following steps: S1. Real-time monitoring of various connected sensors, including LiDAR, camera, IMU and visual odometry, receiving and analyzing the reliability and integrity of sensor data, and outputting the health score of each sensor and the overall health score of the SLAM system. S2. Check the spatiotemporal consistency between sensor data, determine if there is a sensor fault, and output the detection results; S3. Based on the detection results, classify the sensor faults into transient faults, continuous faults, and progressive faults, and take corresponding isolation measures. S4. Dynamically adjust sensor fusion weights to adjust the impact of faulty and normal sensors on the SLAM system; S5. When a sensor malfunctions, an alarm is triggered and the fault information is recorded.

2. The SLAM method with self-diagnostic function according to claim 1, characterized in that, Step S1 executes the lidar health detection process, which specifically includes the following steps: Scan frequency detection: Read the current rotational speed from the radar status message. Compared with nominal value Comparison, if This was determined to be an abnormal frequency. Point cloud density statistics: Count the number of points in each frame of the point cloud. The theoretical expected number of points for the distance range estimation from the environment. Calculate the density ratio If M consecutive frames satisfy The system determines that there is obstruction or contamination. Distance value distribution and noise level: Statistical analysis of distance values ​​collected from multiple frames within the same angular range, calculating the mean and standard deviation. with standard deviation ,like If the value is 3 times or more than the historical average, it is judged as noise anomaly or optical interference; Health score calculation: Set frequency score Density score Noise score By weight , , Perform a weighted summation, that is .

3. The SLAM method with self-diagnostic function according to claim 2, characterized in that, Step S1 executes the camera health check process, which specifically includes the following steps: Brightness and contrast check: Calculate the mean of the grayscale image. Standard deviation ,like or Determine if the brightness is abnormal; If the value is close to 0, it is determined to be lens occlusion or strong blur. Image sharpness detection: Calculating image edge response energy using the Laplacian operator: Where N is the number of pixels. It is a pixel The Laplace response, if Below the threshold And this continued for several frames, which was determined to be lens contamination or poor focus; Feature point extraction success rate: Feature points were extracted using ORB and SIFT algorithms, and the number of feature points was counted. ,like If the value is 30% lower than the average value during the normal period, it is considered that the data quality has declined. Health score calculation: Set brightness score Clarity rating Feature point scoring The overall score is obtained by weighting and summing the results according to their respective weights. .

4. The SLAM method with self-diagnostic function according to claim 3, characterized in that, Step S1, which executes the IMU health check process, specifically includes the following steps: Output frequency and heartbeat detection: Check whether the time interval between adjacent data packets is within the allowable range. If it is not within the allowable range, it is considered a communication abnormality. Noise variance and bias drift monitoring: By estimating motion results using wheel speed odometers and lidar point clouds, the system determines whether the vehicle is stationary. Through a 1-second sliding window, the mean and standard deviation of acceleration and angular velocity for each axis are calculated. If the standard deviation exceeds the calibration value by 2-3 times and remains so for T1, the noise is considered excessive. If the mean angular velocity deviates from 0 or the mean acceleration deviates too much from gravitational acceleration when stationary, bias drift is identified. Health score calculation: The health score of the IMU is calculated based on the results of noise variance and bias drift assessment. .

5. The SLAM method with self-diagnostic function according to claim 4, characterized in that, The multi-sensor health monitoring module performs a visual odometry health check process, which specifically includes the following steps: Monitor the matching degree between the odometer pulse frequency and the robot's actual speed. If the deviation exceeds 20%, it is judged as mechanical wear or slippage failure. Health score calculation: Calculate the health score of the odometer based on its deviation. .

6. The SLAM method with self-diagnostic function according to claim 5, characterized in that, Step S1 executes the overall health scoring process of the system: By lidar weight ,camera IMU weights Odometer weighting Weighted by 0.15, the overall system health score is generated. + + .

7. The SLAM method with self-diagnostic function according to claim 1, characterized in that, Step S2 specifically includes the following steps: S2.1 Detecting the consistency of visual features between LiDAR and map: Using the current map to predict the occupancy probability distribution obtained by laser scanning, match it with the actual point cloud, calculate the matching cost, and determine whether there is an anomaly in the LiDAR or map based on the calculation result of the matching cost and the change of the LiDAR health score. S2.2 Detecting the consistency between the IMU and the visual odometry: Obtaining the pose increment from the visual odometry. The pose increment is obtained by integration using the IMU. Calculate the difference vector And calculate the norm or Mahalanobis distance, if K consecutive frames Exceeding the threshold If so, then it is considered that there is a consistency problem between the two; S2.3 Model Prediction Detection: Using kinematic models or environmental priors, the sensor output is predicted and compared with the actual observation. If the predicted value exceeds the actual observation value, an alarm is triggered. S2.4 Statistical Methods Detection: Apply statistical methods to monitor changes in the statistical characteristics of sensor data, and perform estimation and residual analysis on the sensor data.

8. The SLAM method with self-diagnostic function according to claim 1, characterized in that, Step S4 specifically includes the following steps: S4.1 Constructing the weight mapping function: For N sensors participating in the fusion, the initial nominal weights are: Real-time weights : in, Assess the health of sensor i. Weight mapping function: In the formula, The failure threshold For health threshold, This is the adjustment coefficient; S4.2, Application of the Fusion Framework: Extended Kalman filtering or error-state Kalman filtering will The reciprocal relationship is reflected in the observation noise covariance matrix. middle, and Proportional to this, the observation noise of sensors with low weight is set to exceed a preset value; Construct a graph optimization framework, and Information moments as corresponding constraints scaling factor, By controlling the weights, the influence of the corresponding constraints on the optimization weight mapping function can be adjusted.

9. The SLAM method with self-diagnostic function according to claim 1, characterized in that, Step S4 includes step S4.3, emergency switching mechanism: When the health score of a single sensor Persistently below If the fault exceeds K frames, it is determined to be a persistent fault. The weights of the corresponding sensor are reset to zero, it is removed from the fusion list, and the redundancy mechanism is triggered to start a backup sensor of the same type. If there is no hardware redundancy, it switches to degrade mode and adjusts the parameters of the normally operating sensor.

10. A SLAM system with self-diagnostic function, characterized in that, include: The multi-sensor health monitoring module is used to monitor multiple connected sensors, including LiDAR, camera, IMU and odometer, in real time, receive and analyze the reliability and integrity of sensor data, and output the health scores of each sensor and the overall health score of the SLAM system. The self-diagnosis and fault detection mechanism module is used to check the spatiotemporal consistency between sensor data, determine whether there is a sensor fault, and output the detection results. The fault classification and isolation module is used to classify sensor faults according to the detection results, including transient faults, continuous faults, and progressive faults, and to take corresponding isolation measures. The adaptive strategy adjustment module is used to dynamically adjust the sensor fusion weights and adjust the impact of faulty and normal sensors on the SLAM system. The alarm and recording mechanism module is used to issue an alarm and record fault information when a sensor malfunctions.

Images