Search

CN-122020044-A - SLAM method and system with self-diagnosis function

CN122020044ACN 122020044 ACN122020044 ACN 122020044ACN-122020044-A

Abstract

The invention discloses an SLAM method and system with a self-diagnosis function, the method comprises the steps of receiving and analyzing the credibility and the integrity of sensor data, outputting the health scores of all sensors and the overall health score of an SLAM system, checking the space-time consistency among the sensor data, judging whether a sensor fault exists or not, outputting a detection result, classifying the sensor fault according to the detection result, adopting corresponding isolation measures, dynamically adjusting the fusion weight of the sensor, adjusting the influence of a fault sensor and a normal working sensor on the SLAM system, giving an alarm when the sensor breaks down, and recording fault information. The invention can monitor the states of various sensors in real time and perform self-diagnosis, execute a dynamic self-adaptive data fusion strategy, adjust a multi-sensor fusion algorithm in real time according to the health state of the sensors, and ensure the normal operation of the system under the condition of partial sensor failure.

Inventors

  • MIN JIHAI
  • LIU SHUANG
  • BAI YUNBO

Assignees

  • 南京天创智能科技股份有限公司

Dates

Publication Date
20260512
Application Date
20260130

Claims (10)

  1. 1. A SLAM method with self-diagnostic function, comprising the steps of: S1, monitoring a plurality of sensors which are connected in real time, including a laser radar, a camera, an IMU and a visual odometer, receiving and analyzing the credibility and the integrity of sensor data, and outputting the health score of each sensor and the overall health score of an SLAM system; s2, checking space-time consistency among the sensor data, judging whether a sensor fault exists or not, and outputting a detection result; s3, classifying sensor faults according to the detection result, including transient faults, continuous faults and progressive faults, and taking corresponding isolation measures; S4, dynamically adjusting the fusion weight of the sensor, and adjusting the influence of the fault sensor and the normal working sensor on the SLAM system; and S5, when the sensor fails, an alarm is sent out, and meanwhile, failure information is recorded.
  2. 2. The SLAM method with self-diagnosis function according to claim 1, wherein the step S1 executes a lidar health detection procedure, specifically comprising the steps of: scanning frequency detection, namely reading the current rotating speed from a radar status message And nominal value Comparing, if Judging that the frequency is abnormal; counting the point cloud density, namely counting the point cloud point number of each frame Theoretical expected point number from environmental distance range estimation Calculating the density ratio If the continuous M frames meet Judging that shielding or pollution exists; distance value distribution and noise level, namely counting the distance values acquired by multiple frames in the same angle range, and calculating the mean value and standard deviation And standard deviation of If (if) 3 Times or more of the historical average value, and judging that the noise is abnormal or the optical interference; health score calculation, setting frequency score Density scoring Noise scoring According to the weight 、 、 Weighted summation, i.e. 。
  3. 3. The SLAM method with self-diagnosis function according to claim 2, wherein the step S1 performs a camera health check detection procedure, specifically comprising the steps of: Brightness and contrast inspection, calculating the mean value of gray scale images Standard deviation of If (if) Or (b) Judging abnormal brightness if Approaching 0, judging that the lens is blocked or has strong blurring; image sharpness detection, namely calculating image edge response energy by using Laplacian operator: Wherein N is the number of pixels, Is a pixel If the Laplace response of (a) Below a threshold value And continuing for a plurality of frames, judging that the lens is polluted or the focusing is bad; feature point extraction success rate, namely, feature points are extracted by ORB and SIFT algorithms, and the number of the feature points is counted If (if) 30% Lower than the average value in the normal stage, and judging that the data quality is reduced; Calculation of health score, setting of brightness score Sharpness scoring Feature point score And (5) obtaining a comprehensive score by weighting and summing the weights: 。
  4. 4. the SLAM method with self-diagnosis function according to claim 3, wherein the step S1 performs an IMU health check procedure, specifically comprising the steps of: Checking whether the time interval of the adjacent data packets is in an allowable range or not, and if not, regarding the data packets as abnormal communication; Noise variance and bias drift monitoring, namely judging that the vehicle is in a static state through a wheel speed odometer and a laser radar point cloud estimated motion result, calculating the average value and standard deviation of acceleration and angular velocity of each axis through a 1s sliding window, and judging that the noise is too large if the standard deviation exceeds a standard value by 2-3 times and is continued to be T1; Health score calculation based on the determination of noise variance and bias drift, the health score of IMU is calculated 。
  5. 5. The SLAM method of claim 4, wherein the multi-sensor health monitoring module performs a visual odometer health check procedure comprising the steps of: Monitoring the matching degree of the pulse frequency of the odometer and the actual speed of the robot, and judging mechanical abrasion or slipping faults if the deviation degree exceeds 20%; health score calculation, namely calculating the health score of the odometer according to the deviation degree of the odometer 。
  6. 6. The SLAM method with self-diagnosis function according to claim 5, wherein step S1 performs a system overall health scoring process: Weighting by lidar Camera and camera Weight of IMU Weight of odometer Weight=0.15, generating a system overall health score: + + 。
  7. 7. The SLAM method with self-diagnosis function according to claim 1, wherein the step S2 specifically includes the steps of: S2.1, detecting the consistency of the visual characteristics of the laser radar and the map, namely predicting the occupation probability distribution obtained by laser scanning by using the current map, matching with the actual point cloud, calculating the matching cost, and judging that the laser radar or the map is abnormal according to the calculation result of the matching cost and the change condition of the health score of the laser radar; s2.2, detecting consistency of the IMU and the visual odometer, namely obtaining pose increment by the visual odometer Obtaining pose increment by IMU integration Calculating a disparity vector And calculating the norm or mahalanobis distance if there are consecutive K frames Exceeding a threshold value Then consider that there is a consistency problem with both; S2.3, model prediction detection, namely predicting sensor output by using a kinematic model or environment priori, comparing the sensor output with actual observation, comparing a predicted value with the actual observation value, and triggering an alarm when the predicted value exceeds a threshold value; s2.4, detecting by a statistical method, namely monitoring the statistical characteristic change of the sensor data by using the statistical method, and estimating and residual analyzing the sensor data.
  8. 8. The SLAM method with self-diagnosis function according to claim 1, wherein the step S4 specifically includes the steps of: s4.1, constructing a weight mapping function: for N sensors participating in fusion, the initial nominal weight is Real-time weighting : Wherein, the For the health score of sensor i, The weight mapping function is: In the formula, In order to be a failure threshold value, As a health threshold value, the threshold value is set, For adjusting the coefficient; S4.2, fusion framework application: extended Kalman filtering or error state Kalman filtering Is represented by the inverse relation of the observed noise covariance matrix In the process, the And (3) with Setting the observation noise of the sensor with low weight to be more than a preset value in proportion; Constructing a graph optimization framework to Information moment as corresponding constraint Is used for the scaling factor of (a), And adjusting the influence of the corresponding constraint in the optimization weight mapping function by controlling the weight.
  9. 9. The SLAM method with self-diagnosis function according to claim 1, wherein the step S4 includes a step S4.3, an emergency switching mechanism: Health scoring when a single sensor Continuously lower than And if the number of the frames exceeds K, judging that the frames are continuous faults, setting the weight of the corresponding sensor to zero, removing the weight from the fusion list, triggering a redundancy mechanism, starting the sensor of the standby type, switching to a degradation mode if hardware redundancy is not available, and adjusting the parameters of the sensor which normally works.
  10. 10. A SLAM system having a self-diagnosis function, comprising: The multi-sensor health monitoring module is used for monitoring various sensors including a laser radar, a camera, an IMU and an odometer in real time, receiving and analyzing the credibility and the integrity of sensor data, and outputting health scores of the various sensors and the overall health score of the SLAM system; the self-diagnosis and fault detection mechanism module is used for checking the space-time consistency between the sensor data, judging whether a sensor fault exists or not and outputting a detection result; The fault classification and isolation module is used for classifying sensor faults according to the detection result, including transient faults, continuous faults and progressive faults, and taking corresponding isolation measures; The self-adaptive strategy adjustment module is used for dynamically adjusting the fusion weight of the sensor and adjusting the influence of the fault sensor and the normal working sensor on the SLAM system; and the alarm and recording mechanism module is used for sending an alarm when the sensor fails and recording fault information.

Description

SLAM method and system with self-diagnosis function Technical Field The invention belongs to the technical field of SLAM (Simultaneous Localization AND MAPPING, positioning and mapping), and particularly relates to a SLAM method and system with a self-diagnosis function. Background In industrial environments, robots are widely used in the fields of logistics, manufacturing, inspection, and the like. The SLAM technology is used as a core of autonomous navigation of the robot, and can enable the robot to position itself in an unknown environment and construct an environment map. However, industrial environments often have severe conditions of high temperature, high humidity, strong electromagnetic interference, dust, etc., which easily lead to sensor failures such as laser radar contamination, camera blurring, IMU data drift, etc. The conventional SLAM system may cause positioning errors and map construction failures, thereby causing security accidents. The traditional SLAM system architecture lacks a dynamic evaluation mechanism for the health state of a sensor, and when the output of a single sensor is abnormal due to environmental interference, the system cannot effectively identify the change of the data reliability and still continuously process a degradation signal. The accumulated errors of pose estimation are caused, map construction misalignment or navigation path deviation is caused, and finally, a robot collides with an obstacle, enters a dangerous area by mistake or the operation flow is interrupted, so that the production continuity and the equipment safety of an industrial field are seriously threatened. Disclosure of Invention Aiming at the technical problems, the invention provides an SLAM method and an SLAM system with a self-diagnosis function, which can monitor the health states of various sensors in real time and discover the problem of data credibility in time, thereby reducing positioning errors and improving the reliability and safety of the SLAM system. The technical scheme is that in order to achieve the technical purpose, the invention adopts the following technical scheme: a SLAM method with self-diagnosis function comprises the following steps: S1, monitoring a plurality of sensors which are connected in real time, including a laser radar, a camera, an IMU and a visual odometer, receiving and analyzing the credibility and the integrity of sensor data, and outputting the health score of each sensor and the overall health score of an SLAM system; s2, checking space-time consistency among the sensor data, judging whether a sensor fault exists or not, and outputting a detection result; s3, classifying sensor faults according to the detection result, including transient faults, continuous faults and progressive faults, and taking corresponding isolation measures; S4, dynamically adjusting the fusion weight of the sensor, and adjusting the influence of the fault sensor and the normal working sensor on the SLAM system; and S5, when the sensor fails, an alarm is sent out, and meanwhile, failure information is recorded. Preferably, the step S1 executes a laser radar health detection process, and specifically includes the following steps: scanning frequency detection, namely reading the current rotating speed from a radar status message And nominal valueComparing, ifJudging that the frequency is abnormal; counting the point cloud density, namely counting the point cloud point number of each frame Theoretical expected point number from environmental distance range estimationCalculating the density ratioIf the continuous M frames meetJudging that shielding or pollution exists; distance value distribution and noise level, namely counting the distance values acquired by multiple frames in the same angle range, and calculating the mean value and standard deviation And standard deviation ofIf (if)3 Times or more of the historical average value, and judging that the noise is abnormal or the optical interference; health score calculation, setting frequency score Density scoringNoise scoringAccording to the weight、、Weighted summation, i.e.。 Preferably, the step S1 executes a camera health check detection process, and specifically includes the following steps: Brightness and contrast inspection, calculating the mean value of gray scale images Standard deviation ofIf (if)Or (b)Judging abnormal brightness ifApproaching 0, judging that the lens is blocked or has strong blurring; image sharpness detection, namely calculating image edge response energy by using Laplacian operator: Wherein N is the number of pixels, Is a pixelIf the Laplace response of (a)Below a threshold valueAnd continuing for a plurality of frames, judging that the lens is polluted or the focusing is bad; feature point extraction success rate, namely, feature points are extracted by ORB and SIFT algorithms, and the number of the feature points is counted If (if)30% Lower than the average value in the normal stage, and judging that the