Fault detection method and system for mine unmanned vehicle

Abstract

The invention discloses a fault detection method and system for a mine unmanned vehicle, and relates to the field of positioning navigation, and the method comprises the steps: collecting the data of each sensor; according to the laser radar data, a slam point cloud map is constructed through a cartographic algorithm; according to the IMU data and the vehicle speed data, performing fusion through a Kalman filtering algorithm to obtain odometer data; the preprocessed laser radar data, IMU data, vehicle speed data, camera data and odometer data are converted to a slam point cloud map coordinate system through coordinate conversion, fusion is carried out through a Kalman filtering algorithm, and state estimation of the unmanned vehicle is obtained; taking the state estimation of the unmanned vehicle as a predicted value, comparing the predicted value with laser radar data, IMU data, vehicle speed data and camera data, and calculating an observation residual error; and performing unmanned vehicle fault detection according to the observation residual error. Aiming at the low positioning precision of the mine unmanned vehicle in a complex environment in the prior art, the positioning precision is improved through multi-sensor fusion, fault-tolerant switching and the like.

Description

Technical Field

[0001] The present application relates to the field of positioning and navigation, and in particular to a method and system for detecting faults of unmanned vehicles in underground mines. Background Art

[0002] In recent years, as mining has expanded to deeper areas and complex geological conditions, the underground working environment has become increasingly harsh, and safe and efficient mining of mineral resources faces severe challenges. Autonomous driving technology, with its advantages of reducing manpower, improving efficiency and avoiding danger, provides a new way to solve this problem. Applying unmanned vehicles to underground mining, transportation, detection and other scenarios can greatly improve operating efficiency, reduce labor intensity, and protect the lives of workers, which is of great significance to the development of intelligent and unmanned mining.

[0003] However, there are still many technical difficulties in realizing autonomous driving underground. First, the underground tunnels are long and narrow, with many forks, and the geological structure is complex and changeable, which puts higher demands on the environmental perception and positioning capabilities of unmanned vehicles. Secondly, the underground environment is dim, dusty and foggy, and there are dense obstacles. Traditional single sensors are difficult to adapt to, and multi-sensor fusion solutions are urgently needed. Furthermore, due to the limitations of communication and positioning infrastructure, it is difficult for unmanned vehicles to obtain global information in real time, and they can only rely on their own perception to make decisions, which requires extremely high levels of system autonomy and intelligence.

[0004] In response to the above difficulties, the industry has conducted a lot of research. The existing mainstream solution is based on laser radar for simultaneous localization and mapping (SLAM), which achieves autonomous positioning through scan matching, and builds a grid map or point cloud map reflecting the environment for navigation decisions. However, constrained by the complex and changeable environmental conditions underground, the traditional single laser SLAM method still has many shortcomings: first, the positioning accuracy is not high, and it is difficult to meet the requirements of automatic driving for the precise position of the vehicle; second, it is easily interfered by adverse factors, such as the long corridor effect, light changes, dust and fog occlusion, etc., which will lead to matching failures; third, the robustness is insufficient. When the sensor is damaged or blocked, the system is difficult to work normally; fourth, it lacks the ability to adapt to the environment and cannot adjust parameters and strategies according to dynamic conditions. Summary of the invention

[0005] In response to the problem of low positioning accuracy of underground mining unmanned vehicles in complex environments in the prior art, the present application provides a fault detection method for underground mining unmanned vehicles, which improves the positioning accuracy through multi-sensor fusion and fault-tolerant switching.

[0006] The purpose of this application is achieved through the following technical solutions.

[0007] One aspect of the present application provides a fault detection method for an underground mining unmanned vehicle, comprising: S1, collecting data from various sensors, wherein the data from various sensors include laser radar data, IMU data, vehicle speed data, and camera data, and preprocessing the collected data from various sensors; S2, constructing a SLAM point cloud map through a cartographer algorithm based on the preprocessed laser radar data; S3, fusing the preprocessed IMU data and vehicle speed data through a Kalman filter algorithm to obtain odometer data; S4, converting the preprocessed laser radar data, IMU data, vehicle speed data, camera data, and odometer data to a SLAM point cloud map coordinate system through coordinate conversion, and fusing the laser radar data, IMU data, vehicle speed data, camera data, and odometer data after coordinate conversion through a Kalman filter algorithm to obtain a state estimate of the unmanned vehicle; S5, using the unmanned vehicle state estimate as a predicted value, comparing it with the preprocessed laser radar data, IMU data, vehicle speed data, and camera data, and calculating an observation residual; and performing unmanned vehicle fault detection based on the observation residual.

[0008] Among them, IMU (Inertial Measurement Unit) is an inertial sensor widely used in unmanned vehicles, consisting of an accelerometer and a gyroscope. IMU outputs the acceleration and angular velocity data of the vehicle body at a fixed frequency, reflecting the motion state of the vehicle. In this application, the IMU data is pre-processed and input into the Kalman filter together with the vehicle speed data to estimate the odometer posture. In addition, the IMU data is also used as an observation value to compare with the unmanned vehicle state estimate, calculate the observation residual, and use it for fault detection.

[0009] Camera data is a series of image frames collected by the on-board camera, which contains visual information of the environment around the unmanned vehicle. In this application, the camera data is pre-processed by image enhancement, distortion correction, etc., and then input into the ORB-SLAM algorithm for feature point extraction, feature matching, pose estimation and map construction to obtain the pose of the unmanned vehicle in the camera coordinate system. The pose is converted to the SLAM point cloud map coordinate system through external parameter calibration, and is used as the observation value of the Kalman filter to participate in the state estimation of the unmanned vehicle. At the same time, camera data is also used to calculate observation residuals and monitor anomalies in the positioning process.

[0010] Cartographer is an open source 2D / 3D SLAM algorithm developed by Google for LiDAR point cloud data. It uses a graph optimization-based backend, combined with multi-resolution grid maps and loop detection, to build large-scale, high-precision point cloud maps. In this application, the Cartographer algorithm takes pre-processed LiDAR data as input and generates a globally consistent SLAM point cloud map through scan matching, loop detection, graph optimization and other steps. This map serves as a common coordinate system for other sensor data and provides a global reference for unmanned vehicle state estimation.

[0011] The observation residual refers to the difference between the actual observation value of the sensor and the predicted observation value based on the state estimation. In this application, the state estimation of the unmanned vehicle (including position, speed, posture, etc.) is obtained through Kalman filtering as the predicted value. The predicted value is mapped to the sensor space to obtain the predicted observation value. Then, the predicted observation value is compared with the actual observation value of the sensor to calculate the residual. The residual reflects the degree of consistency between the state estimation and the observed data, and is an important indicator for fault detection. When the residual exceeds the set threshold, it indicates that the state of the unmanned vehicle is abnormal, and further diagnosis of the fault type and fault-tolerant measures are required.

[0012] Further, S1, preprocessing the collected sensor data, including: time synchronization processing of laser radar data, IMU data, vehicle speed data and camera data, synchronizing each sensor data to the same timestamp; specifically, laser radar usually has a built-in high-precision timestamp, which can be directly used for time synchronization. For laser radar without timestamp, time synchronization can be achieved by using external trigger signal or matching with the timestamp of other sensors (such as IMU). IMU usually outputs data at a fixed frequency with an accurate timestamp, which can be used as a reference for time synchronization of other sensors. Vehicle speed data usually comes from wheel encoders, which can be time synchronized by external trigger or matching with IMU timestamp. The timestamp of camera data can be provided by the clock inside the camera or by an external trigger signal. For camera data without timestamp, time synchronization can be achieved by matching with the timestamp of IMU data or using visual odometer to calculate the time interval between adjacent frames. Common time synchronization technologies include: synchronization based on hardware trigger (such as PPS signal), synchronization based on network (such as NTP protocol), synchronization based on timestamp matching (such as linear interpolation), etc.

[0013] The laser radar data, IMU data, vehicle speed data and camera data after event synchronization are spatially synchronized, and the data of each sensor is synchronized to the unmanned vehicle coordinate system to obtain the preprocessed sensor data. Specifically, the laser radar data is usually based on its own coordinate system, and it needs to be converted to the unmanned vehicle coordinate system through external parameter calibration. External parameter calibration can be achieved by measuring the relative pose between the laser radar and the unmanned vehicle coordinate system, or by using methods such as feature point matching and optimization. IMU data is usually based on its own coordinate system, and it needs to be converted to the unmanned vehicle coordinate system through external parameter calibration. External parameter calibration can be achieved by measuring the relative pose between the IMU and the unmanned vehicle coordinate system, or by using the physical properties of the accelerometer and gyroscope for optimization calculation. Camera data is usually based on its own coordinate system, and it needs to be converted to the unmanned vehicle coordinate system through external parameter calibration. External parameter calibration can be achieved by measuring the relative pose between the camera and the unmanned vehicle coordinate system, or by using methods such as feature point matching and optimization. Commonly used spatial synchronization technologies include: manual measurement method, calibration method based on feature point matching, joint calibration method based on optimization, etc.

[0014] Further, S2, constructs a SLAM point cloud map through the cartographer algorithm according to the preprocessed lidar data, including: S21, constructs a local map according to the preprocessed lidar data; S22, performs inter-frame matching between the lidar data of the current frame and the local map, estimates the posture of the current frame, and obtains the lidar odometer; S23, uses the lidar data of the current frame as a key frame, performs loop detection, determines whether there is a loop, and obtains the loop detection result; S24, obtains the optimized local map by minimizing the matching residual between the lidar data frames according to the posture estimation result and the loop detection result; S25, constructs a SLAM point cloud map according to the key frames in the optimized local map as vertices and the relative postures between the key frames as edges; S26, post-processes the SLAM point cloud map.

[0015] Among them, pose refers to the position and orientation of an object in three-dimensional space. In slam, pose usually refers to the transformation matrix of sensors such as cameras or lidar relative to a reference coordinate system, which includes two parts: rotation and translation. Rotation indicates the direction of the sensor, and translation indicates the position of the sensor. In this application, the pose of the current frame relative to the local map can be estimated through inter-frame matching of lidar data to obtain the lidar odometer. In addition, pose graph optimization is the core step of slam. By minimizing the matching residual, a globally consistent key frame pose and map can be obtained.

[0016] Further, S26, post-processing the SLAM point cloud map includes: downsampling the SLAM point cloud map by a voxel filtering algorithm; removing outliers from the downsampled SLAM point cloud map by a radius filtering algorithm; removing ground points from the SLAM point cloud map after removing outliers by a normal estimation and threshold segmentation method; and filtering noise from the SLAM point cloud map after removing ground points by a statistical filtering algorithm to obtain a final SLAM point cloud map.

[0017] Among them, voxel grid filter is a commonly used 3D point cloud downsampling algorithm. It divides the point cloud space into a fixed-size cubic grid (voxel), downsamples the point cloud in each voxel, and obtains a sparser and more uniform point cloud representation. In specific implementation, all point clouds in each voxel are averaged to obtain the coordinates of the voxel center point as the representative point of the voxel. By adjusting the voxel size, the resolution of downsampling can be controlled. In this application, voxel grid filtering is used to reduce the amount of data in the slam point cloud map and improve the efficiency of subsequent processing.

[0018] Radius Outlier Removal is a commonly used algorithm for removing outliers from point clouds. It takes each point as the center and searches for the number of neighboring points within a radius r around it. If the number of neighboring points is less than the set threshold, the point is considered an outlier and removed from the point cloud. Radius filtering can remove noise points that are sparsely distributed and inconsistent with surrounding points in the point cloud, thereby improving the quality of the point cloud. In this application, radius filtering is used to remove outliers in the slam point cloud map in preparation for subsequent ground point segmentation.

[0019] Normal estimation refers to the calculation of the normal vector of each point in the point cloud, which reflects the local geometric characteristics of the point cloud. Common normal estimation methods include PCA (principal component analysis), least squares fitting, etc. Threshold segmentation refers to dividing the point cloud into different areas according to a certain attribute of the point cloud (such as height, normal vector, etc.). In this application, the normal of the slam point cloud map is first estimated, and then a threshold (such as 5°) is set according to the angle between the normal vector and the z-axis, and points with an angle less than the threshold are regarded as ground points to achieve ground point segmentation.

[0020] Statistical Outlier Removal is a commonly used point cloud denoising algorithm. It takes each point as the center and calculates the distance distribution of its surrounding neighborhood points. Assuming that the distance follows a Gaussian distribution, the mean and standard deviation are calculated. Points whose distance exceeds the mean by a certain multiple (such as 1 times) of the standard deviation are regarded as noise points and removed from the point cloud. Statistical filtering can remove randomly distributed outliers in the point cloud and smooth the point cloud surface. In this application, statistical filtering is used to remove noise points in the slam point cloud map to obtain the final high-quality map.

[0021] Further, S3, according to the pre-processed IMU data and vehicle speed data, the Kalman filter algorithm is used to fuse them to obtain the odometer data, including: S31, through the laser radar NDT scan matching algorithm, the laser radar data of the current frame is matched with the slam point cloud map, and the initial posture of the unmanned vehicle under the slam point cloud map is calculated; S32, the pre-processed IMU data is solved to obtain the IMU quaternion reflecting the posture of the unmanned vehicle, and the IMU quaternion represents the hypercomplex extension of the rotation in the three-dimensional space, including a real part and three imaginary parts; S33, according to the IMU quaternion, The posture is solved to obtain the yaw angle yaw of the unmanned vehicle as the heading angle of the unmanned vehicle at the current moment; S34, using the initial posture as a priori estimation, the yaw angle yaw and the preprocessed vehicle speed data as observations, and data fusion is performed through the Kalman filter algorithm to estimate the speed v and angular velocity w of the unmanned vehicle at the current moment; S35, according to the estimated speed v and angular velocity w, the position coordinates (x, y) and yaw angle yaw of the unmanned vehicle at the current moment are calculated through the kinematic model; S36, the position coordinates (x, y) and yaw angle yaw of the unmanned vehicle at the current moment are used as the odometer data at the current moment.

[0022] Among them, NDT (Normal Distributions Transform) is a commonly used lidar point cloud matching algorithm. It divides the point cloud into grids of fixed size, fits the local point cloud distribution to each grid with Gaussian distribution, and aligns the point cloud by minimizing the distance between the two distributions. In specific implementation, the current frame laser point cloud and the map point cloud are first constructed with NDT representations respectively, and then the NDT of the map point cloud is used as the target and the NDT of the current frame point cloud is used as the source. The transformation matrix is ​​iteratively solved through optimization methods such as Newton's method to obtain the pose of the current frame point cloud relative to the map. In this application, NDT scan matching is used to calculate the initial pose of the unmanned vehicle under the slam point cloud map to provide a priori estimation for subsequent Kalman filtering.

[0023] Solving refers to the process of calculating the state information (such as position, speed, attitude, etc.) of the unmanned vehicle based on sensor data through mathematical models and algorithms. In inertial navigation, IMU solving refers to obtaining the speed, position and attitude of the unmanned vehicle through integration operations based on the acceleration and angular velocity data output by the IMU. In visual odometers, camera solving refers to calculating the motion transformation of the camera through algorithms such as PnP and ICP based on feature matching between consecutive image frames. In this application, IMU solving is used to obtain quaternions that reflect the attitude of the unmanned vehicle and provide input for attitude solving.

[0024] Quaternion is a mathematical tool commonly used to represent rotation in three-dimensional space. It consists of a real part and three imaginary parts in the form of , where w is the real part, x, y, z are the imaginary parts, and i, j, k are imaginary units. Compared with Euler angles, quaternions can avoid the universal lock problem and have the advantages of smooth interpolation and rotation synthesis. In inertial navigation, the attitude of the IMU is usually represented by quaternions. By solving the acceleration and angular velocity data, a quaternion reflecting the attitude of the IMU can be obtained. In this application, the IMU quaternion is used as the input of the attitude solution to calculate the heading angle of the unmanned vehicle.

[0025] Attitude solution refers to the process of calculating the attitude (such as pitch angle, roll angle, and heading angle) of the unmanned vehicle based on sensor data. In inertial navigation, attitude solution is usually performed based on the quaternion of the IMU. First, the IMU quaternion is converted into a rotation matrix or Euler angle to obtain the attitude of the IMU relative to the navigation coordinate system (such as the ENU coordinate system). Then, based on the installation relationship between the IMU and the vehicle body coordinate system, the attitude of the unmanned vehicle relative to the navigation coordinate system is obtained through coordinate transformation. In this application, attitude solution is used to calculate the heading angle (yaw angle) of the unmanned vehicle, which is one of the observation quantities of the Kalman filter and participates in the estimation of velocity and angular velocity.

[0026] Further, S4, through coordinate transformation, the pre-processed lidar data, IMU data, vehicle speed data, camera data, and odometer data are transformed into the SLAM point cloud map coordinate system, and the laser radar data, IMU data, vehicle speed data, camera data, and odometer data after coordinate transformation are fused through the Kalman filter algorithm to obtain the state estimation of the unmanned vehicle, including: S41, according to the pre-processed IMU data, vehicle speed data and odometer data, they are transformed into the SLAM point cloud map coordinate system to obtain the IMU attitude, speed and odometer posture of coordinate system 1; S42, through the ORB-SLAM algorithm Process the camera data to obtain the unmanned vehicle posture in the camera coordinate system, and convert the unmanned vehicle posture in the camera coordinate system to the SLAM point cloud map coordinate system; S43, match the laser radar data with the SLAM point cloud map through the laser radar NDT scan matching algorithm to obtain the unmanned vehicle posture in the laser radar coordinate system, and convert the unmanned vehicle posture in the laser radar coordinate system to the SLAM point cloud map coordinate system; S44, use the odometer posture obtained in S41 as a priori estimation, and establish the unmanned vehicle state vector and state transfer matrix according to the IMU posture and speed of S41. The unmanned vehicle state vector includes position (x, y), heading angle θ, speed and angular velocity w; S45, taking the IMU posture of S41 and the angular velocity w of S44 as input, using the state transfer matrix of S44, and through the prediction step of the Kalman filter algorithm, the prior estimate of the state of the unmanned vehicle at the current moment is obtained; S46, taking the posture of the unmanned vehicle in the camera coordinate system of S42, the posture of the unmanned vehicle in the lidar coordinate system of S43, and the odometer posture of S41 as observation quantities, through the update step of the Kalman filter algorithm, the prior estimate of S45 is corrected to obtain the posterior estimate of the state of the unmanned vehicle at the current moment; S47, taking the posterior estimate of S46 as the prior estimate of the Kalman filter algorithm at the next moment, repeating S45 and S46 to perform state estimation and data fusion.

[0027] Among them, ORB-SLAM is a commonly used visual SLAM algorithm, which performs feature extraction, feature matching, pose estimation and map construction based on ORB (Oriented FAST and Rotated BRIEF) feature points. Its main steps include: extracting ORB feature points for each frame of image and calculating descriptors; matching the feature points of the current frame with the previous frame and the local map; estimating the pose of the current frame through PnP and other methods based on the matching point pairs; inserting the current frame into the local map according to the pose, and performing local optimization; performing loop detection and global optimization regularly to eliminate accumulated errors. In this application, ORB-SLAM is used to process camera data to obtain the pose of the unmanned vehicle in the camera coordinate system as one of the observations of the Kalman filter.

[0028] The state vector refers to a set of variables that describe the state of the unmanned vehicle, usually including information such as position, speed, and posture. In this application, the state vector of the unmanned vehicle is defined as X = [ x , y , θ , v x , v y , w ] T , where (x, y) represents the position of the unmanned vehicle in the SLAM point cloud map coordinate system, θ represents the heading angle, and They represent the longitudinal and lateral velocities in the vehicle coordinate system, respectively, and w represents the angular velocity of the heading. The state vector fully describes the motion state of the unmanned vehicle and is the basis for the Kalman filter to perform state estimation and prediction. At each moment, the Kalman filter algorithm updates the state vector based on the observed value to obtain the optimal estimate.

[0029] The state transfer matrix is ​​a key concept in Kalman filtering, which is used to describe the changing relationship between the state vector at two consecutive moments. It maps the state vector at the previous moment to the current moment to form a priori estimation. For the motion of an unmanned vehicle, the state transfer matrix can be constructed based on the kinematic model.

[0030] Odometer pose refers to the pose of the unmanned vehicle calculated through odometer data, including position and heading angle. In this application, the odometer data is obtained by fusing the IMU and vehicle speed data, reflecting the cumulative movement of the unmanned vehicle relative to the initial position. By converting the odometer data to the slam point cloud map coordinate system, the odometer pose can be obtained, which is used to represent the approximate position of the unmanned vehicle in the global map. Due to the cumulative error of the odometer, its accuracy is relatively low, but its continuity is good. In Kalman filtering, the odometer pose is usually used as a priori estimation to provide a rough position reference for other sensors, which helps to accelerate filter convergence.

[0031] Further, S5, the unmanned vehicle state estimation is used as the predicted value, and compared with the preprocessed lidar data, IMU data, vehicle speed data and camera data to calculate the observation residual; the unmanned vehicle fault detection is performed according to the observation residual, including: S51, the posterior estimation of the unmanned vehicle state at the current moment obtained in S46 is used as the predicted value, and the preprocessed lidar data, IMU data, vehicle speed data or camera data in S1 is used as the observed value z; S52, the residual between the observed value z and the predicted value is calculated ; S53, based on the residual And the observation noise covariance matrix R of the corresponding sensor, calculate the residual sum of squares as the innovation amount of the corresponding sensor ; S54, for each sensor, compare the corresponding innovation amount With the preset threshold γ, if is less than or equal to γ, then return to step S51. If If it is greater than γ, it is determined that the corresponding sensor fails, and step S55 is executed; S55, the failure type of the sensor is obtained, and the corresponding fault-tolerant strategy is acquired.

[0032] Further, S55, the fault type of the sensor is obtained, and the corresponding fault-tolerant strategy is acquired, including: when the faulty sensor is a lidar, the unmanned vehicle posture in the lidar coordinate system in the observation measurement in step S46 is deleted; when the faulty sensor is a camera, the unmanned vehicle posture in the camera coordinate system in the observation measurement in step S46 is deleted; when the faulty sensor is an IMU, the speed in S44 is and angular velocity w are set as constants; obtain the fault duration of the sensor, determine whether the fault duration is greater than a threshold, and if so, perform data repair.

[0033] Furthermore, the fault duration of the sensor is obtained, and it is determined whether the fault duration is greater than a threshold value. If so, data repair is performed, including: extracting the posterior estimate of the state of the unmanned vehicle at N moments obtained in step S46 before the sensor fault occurs as an interpolation node, and constructing an n-order Lagrange interpolation polynomial; using the n-order Lagrange interpolation polynomial, inferring the posterior estimate of the state of the unmanned vehicle at M moments within the fault time period, as virtual observation data within the fault time period, and participating in the update step of the Kalman filter algorithm; calculating the condition number cond(P) of the state estimation covariance matrix of the Kalman filter algorithm; when the condition number cond(P) is greater than the threshold value, it indicates that the corresponding filter is damaged, and the user is prompted. Among them, in the Kalman filter, the state estimation covariance matrix P reflects the uncertainty of the state estimation. The condition number cond(P) of P is defined as the ratio of the maximum eigenvalue to the minimum eigenvalue of P, that is, . It is an indicator of the degree of pathological condition of the matrix P. The condition number of P reflects the sensitivity of the Kalman filter to observation noise. When the condition number is large, it means that the eigenvalues ​​of P are unevenly distributed, and some eigenvalues ​​are much larger than other eigenvalues. In this case, the Kalman gain The amplification effect on noise is more obvious, the filtering result is easily affected by noise, and the numerical stability is poor. On the contrary, when the condition number is small, it means that the eigenvalue distribution of P is more uniform. At this time, the Kalman filter is less sensitive to observation noise and has better numerical stability. Ideally, the state estimation covariance matrix P should converge to a stable value, and its condition number should be small and stable. If the condition number continues to increase, it means that the filtering is unstable and the estimation error accumulates too quickly.

[0034] In this application, sensor failure may lead to missing or erroneous observation data, making the Kalman filter state estimation inaccurate. In order to reduce the impact of the failure, virtual observation data is generated by Lagrange interpolation to make up for the missing information. However, the virtual observation data obtained by interpolation is different from the real observation after all, and the introduced noise may affect the stability of the filter.

[0035] Therefore, on the basis of interpolation repair data, it is also necessary to monitor the state of the Kalman filter. The health of the filter is judged by calculating the condition number cond(P) of the state estimation covariance matrix P. If cond(P) exceeds the preset threshold, it means that the noise introduced by the current interpolation data is too large, resulting in unstable filtering and ill-conditioned covariance matrix, and the user needs to be prompted that the filter is damaged. This filter monitoring method based on condition number can effectively detect the adverse effects introduced in the interpolation repair process, avoid state estimation divergence, and improve the fault tolerance of the system. At the same time, it also provides users with feedback on the health status of the filter, so as to facilitate timely handling of potential failure risks.

[0036] Another aspect of the present application also provides a fault detection system for an underground mining unmanned vehicle, which is used to execute a fault detection method for an underground mining unmanned vehicle of the present application.

[0037] Compared with the prior art, the advantages of this application are:

[0038] Using IMU, LiDAR, camera, odometer and other multi-source heterogeneous sensors, unified representation and fusion processing of sensor data are achieved through time synchronization and spatial coordinate conversion. The cartographer algorithm is used to build a high-precision slam point cloud map as a global position reference, and combined with a two-level Kalman filter algorithm, the visual odometer, IMU, speedometer and LiDAR positioning results are deeply integrated, overcoming the defects of a single sensor being susceptible to environmental interference and data discontinuity, giving full play to the complementary advantages of multiple sensors, and effectively improving the positioning accuracy and continuity of the unmanned vehicle.

[0039] Traditional positioning methods rely on pre-built environmental maps or external auxiliary facilities, and lack adaptability and flexibility. SLAM technology uses the robot's own sensors to perceive the surrounding environment, and uses the closed-loop recursion of motion estimation and environmental perception to synchronously build a map reflecting the environment during the movement, and locate its own position in the map in real time. It is an effective method for autonomous map construction and positioning. Combining it with graph optimization theory can further improve the global consistency and positioning accuracy of the map. LiDAR has become an ideal sensor for SLAM with its active detection, high precision, long distance and high stability. The generated laser point cloud map has rich spatial structure information, which can not only be used for positioning, but also can be directly applied to navigation decisions. Obtain the motion trajectory of the planned unmanned vehicle to achieve true autonomous navigation.

[0040] Due to the complexity and variability of the underground environment, unmanned vehicles will inevitably encounter sensor damage or data anomalies. The traditional single perception architecture lacks effective fault diagnosis and fault-tolerant control means. Once the key sensor fails, it is very easy to lead to serious consequences such as positioning and tracking failure and misjudgment of decisions. This application uses SLAM positioning as a reference standard for fault diagnosis, and realizes fault detection and isolation through multi-sensor residual analysis, overcoming the limitations of single perception. At the same time, a multi-level and multi-means fault-tolerant control scheme is constructed using redundant sensors, virtual observation value interpolation and other methods. Even if a local sensor fails, the system can still automatically switch to safe mode to ensure that the unmanned vehicle is in a controlled state. The setting of covariance indicators and thresholds provides a powerful monitoring means for filtering failure.

[0041] The system automatically takes corresponding measures based on the fault diagnosis results: when the fault time is short, the system maintains normal operation by switching redundant sensors and using virtual data; when the fault duration exceeds the threshold, the trajectory prediction model based on the historical state is constructed through Lagrange interpolation, and the predicted value is used as a priori input fusion filter to extend the autonomous working time of the unmanned vehicle. On the other hand, the system uses the covariance threshold judgment to identify the failure of the filter and request manual intervention when necessary. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] The present application will be further described in the form of exemplary embodiments, which will be described in detail by the accompanying drawings. These embodiments are not restrictive, and in these embodiments, the same number represents the same structure, wherein:

[0043] Figure 1 is an exemplary flow chart of a method for detecting faults of an unmanned underground mining vehicle according to some embodiments of the present application;

[0044] Figure 2 is an exemplary flow chart of sensor data synchronization according to some embodiments of the present application;

[0045] Figure 3 It is an exemplary flow chart of obtaining odom information by fusing vehicle speed information and imu information as shown in some embodiments of the present application. DETAILED DESCRIPTION

[0046] The method and system provided in the embodiments of the present application are described in detail below with reference to the accompanying drawings.

[0047] like Figure 1As shown, S1, collects data from various sensors, wherein the data from various sensors include lidar data, IMU data, vehicle speed data and camera data, and preprocesses the collected sensor data; S2, constructs a SLAM point cloud map through the cartographer algorithm based on the preprocessed lidar data; S3, fuses the preprocessed IMU data and vehicle speed data through the Kalman filter algorithm to obtain odometer data; S4, converts the preprocessed lidar data, IMU data, vehicle speed data and camera data, as well as the odometer data, to the SLAM point cloud map coordinate system through coordinate conversion, and fuses the coordinate converted lidar data, IMU data, vehicle speed data and camera data, as well as the odometer data through the Kalman filter algorithm to obtain the state estimation of the unmanned vehicle; S5, uses the unmanned vehicle state estimation as a predicted value, compares it with the preprocessed lidar data, IMU data, vehicle speed data and camera data, and calculates the observation residual; and performs unmanned vehicle fault detection based on the observation residual.

[0048] like Figure 2 As shown, specifically, S1 pre-processes the collected sensor data. First, a time synchronization mechanism is introduced to address the inconsistency of timestamps of data collected by different sensors. Through methods such as timestamp alignment and interpolation, the data of sensors such as lidar, IMU, vehicle speed, and camera are unified to the same time reference. This step ensures the consistency of various data in the time dimension, eliminates the estimation error caused by asynchrony, and is the prerequisite for subsequent data fusion.

[0049] Secondly, on the basis of time synchronization, the spatial coordinate system is further unified. Due to the different installation positions and orientations of each sensor, its measurement data is located in its own local coordinate system and cannot be directly compared and fused. Therefore, it is necessary to unify the data in each sensor coordinate system into the reference coordinate system of the unmanned vehicle itself through coordinate transformation. This process needs to consider the installation parameters of the sensor, including relative position and relative posture, and use mathematical tools such as rotation matrix and translation vector to realize the conversion between different coordinate systems. After spatial synchronization, the data of each sensor is uniformly represented in a framework with the vehicle body as the reference, which is convenient for registration and fusion processing.

[0050] Through the above-mentioned spatiotemporal synchronization preprocessing, the originally independent and heterogeneous sensor data are integrated into a unified spatiotemporal framework, and the temporal and spatial consistency of the data is guaranteed. Although this process is simple, it is the basis and premise of multi-sensor fusion. Only in a unified reference system can the information from different sensors be effectively aligned and used in subsequent links such as state estimation and decision planning. The accuracy of synchronization directly affects the effect of fusion positioning.

[0051] S2, based on the pre-processed lidar data, the SLAM point cloud map is constructed through the cartographer algorithm. First, before data collection, necessary sensor calibration and time synchronization are required. The relative pose relationship between the lidar, IMU and vehicle coordinate system is determined through calibration, laying the foundation for data registration. The PTP time synchronization strategy is adopted to synchronize the clocks of each sensor and ensure the consistency of the timestamp. The collected multi-sensor data is stored as a bag package for subsequent offline processing.

[0052] The cartographer algorithm uses preprocessed lidar data as input and realizes inter-frame pose estimation and local map construction through scan matching. Specifically, each frame of laser data is matched with the current local map to estimate its own pose and form a laser odometer trajectory. This process makes full use of the spatial constraint relationship between consecutive frames and realizes the recursive update of inter-frame pose. At the same time, by adding new frame data to the local map, the coverage of the map is continuously expanded so that it can reflect the environmental characteristics around the vehicle.

[0053] During the local map construction process, the algorithm will select some keyframes for loop detection. The purpose of loop detection is to determine whether the current location is a place that has been visited before, that is, whether there is a closed loop. By comparing the similarity between the current keyframe and the historical keyframe, potential loop candidates can be found. If a loop is successfully detected, it means that there is a closed constraint in the map, which can be used to optimize global consistency.

[0054] When a loop is detected, the algorithm introduces closure constraints during the graph optimization process to optimize the global pose graph and map. The goal of graph optimization is to minimize the inter-frame matching residual, that is, to make the relative pose relationship between all frames the most coordinated and consistent, and to reduce the cumulative error. By constructing an error function and using optimization algorithms such as G2O and CERES for iterative solution, the optimized pose and map corrected by the closure constraints can be obtained. This process effectively eliminates the cumulative errors introduced by factors such as odometer drift and mismatching, and greatly improves the global consistency of the SLAM results.

[0055] After completing local map construction and global optimization, the algorithm further stitches a series of local maps into a global map covering the entire environment. With the optimized keyframe poses as vertices and the relative poses between keyframes as edges, a global pose graph is constructed to reflect the motion trajectory of the unmanned vehicle. At the same time, each local point cloud map is registered and stitched according to the global pose graph to generate the final global point cloud map. This map covers the entire area explored by the unmanned vehicle, contains rich environmental geometry information, and is uniformly stored in pcd format for subsequent loading and application.

[0056] Finally, the generated point cloud map is post-processed to improve the map accuracy and usage efficiency. In order to solve the problem of large amount of original point cloud data and high redundancy, the voxel grid filtering algorithm is used for downsampling. The algorithm divides the three-dimensional space into cube grids of uniform size, merges the point clouds in each grid, and replaces them with a single point at the center of the voxel, thereby greatly reducing the number of point clouds. By adjusting the voxel size, the accuracy and compression rate of downsampling can be flexibly controlled, which significantly reduces the data volume and computational load while ensuring the map resolution.

[0057] For abnormal measurement values ​​and outliers, the radius filtering algorithm is used to remove them. The algorithm takes each point as the center and determines whether the point is an outlier by searching the number of neighborhood points within a preset radius around it. If the number of neighborhood points is lower than the threshold, the point is considered to be noise or an outlier and is removed. By setting the search radius and threshold reasonably, isolated points and erroneous measurements can be effectively removed, improving the overall quality of the point cloud.

[0058] In order to highlight the obstacle information in the map, the normal estimation and threshold segmentation methods are used to remove ground points. By estimating the normal of the point cloud, the normal vector of each point is obtained to reflect the orientation of the local surface. Using the horizontal characteristics of the ground, the threshold of the angle between the normal vector and the z-axis is set, and the points with an angle less than the threshold are marked as ground points. Then, through methods such as clustering segmentation, the ground points are separated from the point cloud to obtain a point cloud map mainly containing obstacles, laying the foundation for subsequent obstacle avoidance decisions.

[0059] For the residual high-frequency noise, a statistical filtering algorithm is used for denoising. Common statistical filtering methods such as mean filtering and median filtering perform statistical analysis on the local neighborhood of the point cloud, and replace the coordinates of the center point with the mean or median of the neighborhood to weaken the influence of random noise. After the above series of filtering, segmentation, and denoising, a high-precision point cloud map with high signal-to-noise ratio and prominent features is finally obtained.

[0060] This application combines the Cartographer algorithm with a multi-sensor fusion solution, fully utilizing the high-precision, long-distance perception advantages of lidar, overcoming the limitations of traditional visual SLAM such as lighting changes and texture loss, and achieving stable mapping in extreme environments.

[0061] like Figure 3 As shown in S3, this application uses IMU data and vehicle speed data to achieve odometer estimation of the unmanned vehicle through the Kalman filter algorithm. The entire process uses the initial posture obtained by laser radar NDT scanning matching as a priori, combines the IMU posture information and vehicle speed measurement value, recursively updates the position and heading angle of the unmanned vehicle, and outputs the result as odometer data.

[0062] First, the laser data of the current frame is registered with the pre-built slam point cloud map through the lidar NDT scan matching algorithm. NDT (Normal Distributions Transform) is a point cloud matching algorithm that divides the point cloud into grids of fixed size, fits the local point cloud distribution to each grid with a Gaussian distribution, and registers the point cloud by minimizing the distance between the two Gaussian distributions. Compared with algorithms such as ICP, NDT is insensitive to local optimality and converges faster.

[0063] In the matching process, the laser point cloud of the current frame is first preprocessed by filtering, downsampling, etc. to extract valid feature points. Then, the predicted pose is used as the initial value to search for the pose that best matches the slam map in the local area. Through iterative optimization, the distance metric between the two point clouds is minimized to obtain the optimal pose estimate, which is the position (x, y) and yaw angle yaw of the unmanned vehicle under the global map. This pose is used as the initial value of the odometer and the prior estimate of the Kalman filter, which corrects the accumulated error of the laser odometer and provides a global reference for subsequent calculations.

[0064] Secondly, the attitude of the pre-processed IMU data is solved. The IMU outputs quaternions at a fixed frequency (such as 100Hz), which represent the three-dimensional rotation of the IMU device relative to the reference coordinate system (such as the northeast sky). It consists of a real part w and three imaginary parts x, y, z, satisfying Compared to Euler angles, quaternions do not have the universal lock problem and can continuously represent arbitrary rotations.

[0065] Known IMU quaternion , the three Euler angles of roll, pitch, and yaw can be calculated by the following formula: , , , where the yaw angle yaw represents the rotation angle of the IMU relative to the reference system in the horizontal plane, and represents the heading angle of the unmanned vehicle. Due to the high frequency of the IMU, the yaw angle can reflect the changes in the heading of the unmanned vehicle in real time, making up for the low frequency of the laser odometer. Using yaw as the observation quantity of the Kalman filter can provide high-frequency feedback for the heading of the unmanned vehicle and improve the dynamic response of the odometer.

[0066] It should be noted that the IMU quaternion gives the rotation relative to the reference coordinate system, and the coordinate transformation is required to convert it to the base_link system with the unmanned vehicle as the origin. This transformation can be obtained by calibrating the rotation matrix or the corresponding quaternion accomplish: ,in, It is a four-element in the base_link system. is the quaternion in the imu coordinate system, yes The inverse element of .

[0067] The yaw angle obtained by solving q_base is the observation required for odometer estimation. In addition, due to errors such as zero bias and scale factor in IMU, the integration of quaternion will drift, which needs to be corrected during the filtering process.

[0068] In this application, laser NDT matching and IMU solution provide pose prior and heading observation for odometer estimation from global and local perspectives respectively. NDT matching overcomes the drift of laser odometer, and IMU solution makes up for the delay of laser odometer. The complementary fusion of the two not only ensures the global reference of estimation, but also improves the dynamic response, which is the key to improving the accuracy of odometer.

[0069] After obtaining the initial pose matched by laser NDT and the yaw angle solved by IMU, this application uses the Kalman filter algorithm to fuse them with the vehicle speed measurement value to obtain the speed and angular velocity estimation of the unmanned vehicle, and recursively calculate the position coordinates through the kinematic model, and finally output the odometer data.

[0070] Kalman filtering is a recursive optimal estimation algorithm that continuously corrects the estimated value of the state quantity through two steps: prediction and update. In this application, the state quantity X includes the position coordinates (x, y), yaw angle yaw, speed v and angular velocity w of the unmanned vehicle, and the observation quantity Z includes the yaw angle yaw calculated by the IMU and the speed v measured by the speedometer.

[0071] In the prediction stage, based on the estimated value at the previous moment t-1 and control input , use the kinematic model to predict the state quantity at the current time t : , where A is the state transfer matrix, which maps the state quantity at the previous moment to the current moment; B is the control input matrix, which acts on the control quantity U(t) on the state quantity. Assuming uniform motion, A and B can be constructed by the following kinematic equations:

[0072] ; ; ; ; ; While predicting, it is also necessary to calculate the predicted covariance matrix of the state quantity , as a measure of the uncertainty of the estimate: , where Q is the process noise covariance matrix, which represents the impact of model error.

[0073] In the update phase, the observed value Z(t) at the current time t is used to update the predicted value Correction is performed to obtain the posterior estimate X(t). First, the error between the observed value and the predicted value is calculated, that is, the new information vector y(t): Where H is the observation matrix, which maps the state quantity to the observation space. In this application, H is defined as: H = [ 0 0 1 0 0 0 0 0 1 0 ] It means that the observed quantities yaw and v correspond to the third and fourth components of the state quantity respectively.

[0074] Then, the Kalman gain K(t) is calculated to determine the contribution of the new information vector y(t) to the posterior estimate: , where R is the observation noise covariance matrix, which represents the influence of sensor error.

[0075] Finally, the Kalman gain K(t) is used to update the posterior estimate X(t) and covariance matrix P(t) of the state quantity: , , so far a filtering cycle is completed, X(t) is used as the prior estimate of the next moment, and the prediction and update process is repeated.

[0076] According to the velocity v(t) and angular velocity w(t) obtained by Kalman filtering, the current position is calculated using the kinematic equation and yaw angle :

[0077] , ; ; Among them, dt is the sampling period, and the position (x, y) and yaw angle yaw are the odometer outputs at the previous moment.

[0078] The updated position and yaw angle Published as the odometer at the current moment, that is, the tf transformation of odom->base_link. This odometer combines the information of laser NDT, IMU and speedometer, and recursively estimates the pose under global reference, with better accuracy than a single sensor.

[0079] The Kalman filter algorithm makes full use of the advantages of each sensor, smoothly integrates the global pose, heading angle and velocity information, and dynamically weighs the credibility of the predicted value and the observed value, which not only ensures the continuity of the estimation, but also takes into account the correction effect of the observation, thereby obtaining the optimal odometer estimate.

[0080] This application makes full use of the high-frequency attitude information of the IMU and the speed information of the speedometer, making up for the insufficient update frequency of the laser radar odometer and achieving smooth and continuous odometer estimation. The Kalman filter algorithm processes noise in the form of probability, obtains the optimal estimate, and is adaptive and robust. Combining global laser matching with local IMU tracks effectively suppresses cumulative errors and improves odometer accuracy.

[0081] S4, through coordinate transformation, converts the pre-processed lidar data, IMU data, vehicle speed data, camera data, and odometer data to the SLAM point cloud map coordinate system, and fuses the converted lidar data, IMU data, vehicle speed data, camera data, and odometer data through the Kalman filter algorithm to obtain the state estimation of the unmanned vehicle.

[0082] Specifically, first, the pre-processed IMU data, vehicle speed data, and odometer data are uniformly converted to the slam point cloud map coordinate system. The IMU provides the attitude information of the unmanned vehicle, including the three Euler angles of roll, pitch, and yaw. The vehicle speed data contains the linear velocity v and angular velocity w of the unmanned vehicle. The odometer data contains the position (x, y) and heading angle θ of the unmanned vehicle. Through coordinate transformation, the IMU attitude, speed, and odometer posture in the unified coordinate system are obtained, laying the foundation for subsequent fusion.

[0083] Secondly, this application adopts two mainstream algorithms, ORB-SLAM and NDT, to process camera images and laser point clouds respectively, and obtain their poses under the SLAM map, which provides important observation information for subsequent Kalman filtering. For camera data, the ORB-SLAM algorithm is used for processing, and for each frame of the image, the ORB (Oriented FAST and RotatedBRIEF) feature descriptor is used to extract and describe feature points. ORB features are rotationally invariant and scale invariant, and have high computational efficiency. The ORB feature points extracted from the current frame are matched with the feature points in the previous frame and the local map to obtain feature point pairs. The matching process uses the FLANN (Fast Library for Approximate Nearest Neighbors) algorithm, combined with the Hamming distance to measure similarity. Based on the feature point pairs, the PnP (Perspective-n-Point) algorithm is used to estimate the pose transformation of the current frame camera relative to the previous frame and the local map. The PnP problem solves the camera pose between 3D-2D point pairs, and the robustness can be improved by methods such as RANSAC. According to the estimated camera pose, insert the ORB feature points and keyframes of the current frame into the local map, and update the map points and co-visual map. After the local map reaches a certain size, trigger global optimization. Through graph optimization methods (such as g2o), minimize the reprojection error, optimize the poses of all keyframes and the coordinates of map points, and obtain globally consistent trajectories and maps. By calculating the similarity between the current frame and the historical keyframes, determine whether a closed loop occurs. If a closed loop is detected, add a closed loop constraint to the graph optimization to eliminate trajectory drift.

[0084] By processing the camera image sequence through ORB-SLAM, the camera's pose at each moment (i.e., rotation and translation relative to the initial position) and a sparse feature point map can be obtained. Considering the pose deviation between the slam point cloud map coordinate system and the camera coordinate system, it is necessary to solve the extrinsic parameter transformation between the two coordinate systems through eye-in-hand calibration. Hand-eye calibration usually uses the AX=XB method proposed by Ze Zhang et al., where A is the pose transformation in the camera coordinate system, B is the pose transformation in the slam map coordinate system, and X is the extrinsic parameter to be determined. Solving the linear equation system AX=XB through singular value decomposition (SVD) can obtain the rotation matrix R and translation vector t, and transform the camera pose to the slam map.

[0085] For the laser radar point cloud data, the NDT (Normal Distribution Transform) algorithm is used for processing. NDT is a point cloud matching method based on probability distribution. It divides the point cloud into grids of fixed size, fits the local point cloud distribution with Gaussian distribution for each grid, and aligns the point cloud by minimizing the distance between the two distributions. The original laser point cloud is filtered (such as voxel filtering, statistical filtering, etc.) and downsampled to remove noise and outliers and reduce the amount of data. The point cloud is divided into three-dimensional grids of fixed size (such as cubes), and Gaussian distribution is fitted to each grid to obtain parameters such as mean and covariance to characterize the distribution characteristics of the local point cloud. With the slam map point cloud as the target and the current frame point cloud as the source, the coordinate transformation from the source point cloud to the target point cloud is obtained by minimizing the distance between the two NDT distributions. The distance metric uses a scoring function, such as the negative log-likelihood of the point to the distribution. According to the coordinate transformation obtained by NDT matching, the pose (rotation and translation) of the current frame laser point cloud relative to the slam map is calculated. To improve the matching accuracy, an iterative optimization strategy, such as the Gauss-Newton method, can be used.

[0086] Similar to the camera, there is also an external parameter deviation between the laser radar coordinate system and the slam map coordinate system. Through methods such as calibrating the target plate, the corresponding point clouds in the two coordinate systems are collected, the rotation and translation equations are constructed, and the least squares method is used to solve them to obtain the external parameter transformation matrix, and the posture obtained by NDT matching is converted to the slam map.

[0087] After the camera pose obtained by ORB-SLAM and the lidar pose obtained by NDT are unified into the SLAM map coordinate system, they can be used as the observation quantity of the Kalman filter and fused with other sensor data such as IMU and odometer to obtain a high-precision state estimation of the unmanned vehicle. ORB-SLAM and NDT provide pose observations from the perspectives of vision and point cloud respectively, and the complementary fusion ensures the accuracy of the estimation and improves the robustness of the system.

[0088] Then, using the odometer pose as a priori estimation, the IMU attitude and velocity as control input, and based on the simplified constant speed kinematic model, the unmanned vehicle state vector X and state transfer matrix A are established: X = [ x , y , θ , v x , v y , w ] T ; A = [ 1 0 0 dt cos θ dt sin θ 0 0 1 0 − dt sin θ dt cos θ 0 0 0 1 0 0 dt 0 0 0 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 ]

[0089] Where dt is the sampling time.

[0090] Next, using the IMU attitude and angular velocity w, through the state transfer matrix A, and the prediction step of the Kalman filter, we can get the prior estimate of the current state of the unmanned vehicle: ; , where P is the state covariance matrix and Q is the process noise covariance matrix.

[0091] Finally, the camera pose, lidar pose and odometer pose are used as observations to construct the observation equation: , where Z is the observation vector, H is the observation matrix, and R is the observation noise. H = [ 1 0 0 0 0 0 0 1 0 0 0 0 0 0 1 0 0 0 ] , indicating that the observations only contain position (x, y) and heading angle θ.

[0092] Through the update step of the Kalman filter, the prior estimate is corrected based on the observed value: ; ; Where K is the Kalman gain and I is the identity matrix.

[0093] The updated state estimate As the prior estimate for the next moment, the prediction and update process is repeated to achieve recursive estimation of the state of the unmanned vehicle and dynamic fusion of multi-sensor data. The estimation results include the position (x, y), heading angle θ, speed and angular velocity w, which can be used for tasks such as positioning, navigation, and control.

[0094] The Kalman filter method used in this application uses odometer posture as a priori, camera, lidar, and IMU data as observations, and combines it with a constant speed kinematic model to achieve seamless fusion of multiple on-board sensors. This method can flexibly add or remove sensors and can be easily expanded to more complex motion models.

[0095] S5. Based on Kalman filtering, this application adds fault detection and fault tolerance mechanisms for unmanned vehicles, which improves the robustness and reliability of the system. By calculating the residual between the observed value and the predicted value, an innovative quantity index is constructed to achieve real-time monitoring and diagnosis of sensor faults. At the same time, corresponding fault tolerance strategies are designed for different fault types to ensure the continuous operation of the unmanned vehicle in the event of sensor failure.

[0096] First, the current state estimate of the unmanned vehicle obtained by Kalman filtering is used as the predicted value, and the preprocessed sensor data is used as the observed value to calculate the residual between them: ,in, is the residual vector, z is the observation value, H is the observation matrix, is the state posterior estimate. Then, according to the residual And the observation noise covariance matrix R of the corresponding sensor, calculate the residual sum of squares, and get the innovation amount of the sensor : , innovation It reflects the degree of difference between the observed value and the predicted value and obeys the chi-square distribution.

[0097] Next, for each sensor, compare the innovation and the preset threshold γ. If Less than or equal to γ, it means that the observed value is consistent with the predicted value and the sensor is working properly; if If it is greater than γ, it means that the observed value deviates from the predicted value and the sensor may be faulty. The threshold γ can be selected according to the confidence and degrees of freedom of the chi-square distribution to balance missed detection and false detection.

[0098] When a sensor failure occurs, the system first obtains the fault type and adopts the corresponding fault tolerance strategy: For a lidar failure, delete the lidar pose in the observation and rely only on other sensors. For a camera failure, delete the camera pose in the observation and rely only on other sensors. For an IMU failure, The angular velocity w is set to a constant and the state prediction is suspended. Then, the system obtains the fault duration of the sensor and determines whether it exceeds the threshold. If it exceeds the threshold, it means that the sensor has failed for a long time and data repair is required to ensure the continuity and stability of the filtering algorithm.

[0099] Data repair is based on Lagrangian interpolation. The specific steps are as follows: Extract the state estimation of the unmanned vehicle N moments before the sensor failure as the interpolation node. Construct the n-th order Lagrangian interpolation polynomial : ,in, is the state estimate of the ith node, is the Lagrange basis function, expressed as: , use the interpolation polynomial L(x) to compensate the state estimates at M moments during the fault period as virtual observations. Substitute the virtual observations into the Kalman filter to participate in the state update and obtain the repaired state estimate. Calculate the condition number of the state estimate covariance matrix P ,If cond(P) is greater than the threshold, it indicates that the repaired filter is unstable, and the user is prompted.

[0100] The fault detection method of the present application is based on the innovative quantity of Kalman filtering, which realizes the real-time monitoring and diagnosis of sensor faults. According to different fault types, fault-tolerant strategies such as switching redundancy, modifying parameters, and data interpolation are designed to maximize the continuous operation of the unmanned vehicle. Lagrange interpolation can use the state estimation before the fault to reasonably compensate for the state during the fault, maintaining the smoothness of the filtering trajectory. The condition number of the covariance matrix provides a quantitative uncertainty indicator for the system, which is convenient for users to monitor the filtering quality. This method is simple to calculate and has strong real-time performance. It gives full play to the advantages of multi-sensor fusion, significantly improves the fault tolerance and reliability of the state estimation of the unmanned vehicle, and is of great significance to the long-term autonomous operation of the unmanned vehicle.

Claims

1. A method for detecting faults of unmanned mining vehicles, characterized in that: include: S1, collecting data from various sensors, wherein the data from various sensors include lidar data, IMU data, vehicle speed data, and camera data, and preprocessing the collected data from various sensors; S2, constructs the SLAM point cloud map through the cartographer algorithm based on the preprocessed lidar data; S3, based on the pre-processed IMU data and vehicle speed data, the odometer data is obtained by fusing them through the Kalman filter algorithm; S4, converts the pre-processed lidar data, IMU data, vehicle speed data, camera data, and odometer data to the SLAM point cloud map coordinate system through coordinate conversion, and fuses the laser radar data, IMU data, vehicle speed data, camera data, and odometer data after coordinate conversion through the Kalman filter algorithm to obtain the state estimation of the unmanned vehicle; S5, using the estimated state of the unmanned vehicle as the predicted value, comparing it with the preprocessed lidar data, IMU data, vehicle speed data, and camera data, and calculating the observation residual; Unmanned vehicle fault detection based on observation residuals.

2. The method for detecting faults of unmanned underground mining vehicles according to claim 1, characterized in that: S1, pre-processing the collected sensor data, including: Perform time synchronization processing on lidar data, IMU data, vehicle speed data and camera data, and synchronize the data of each sensor to the same timestamp; The laser radar data, IMU data, vehicle speed data and camera data after event synchronization are spatially synchronized, and the data of each sensor is synchronized to the unmanned vehicle coordinate system to obtain the pre-processed sensor data.

3. The fault detection method for unmanned underground mining vehicles according to claim 1, characterized in that: S2, based on the preprocessed lidar data, builds the SLAM point cloud map through the cartographer algorithm, including: S21, constructing a local map based on the preprocessed lidar data; S22, performing inter-frame matching between the laser radar data of the current frame and the local map, estimating the position and posture of the current frame, and obtaining the laser radar odometer; S23, taking the laser radar data of the current frame as a key frame, performing loop detection, determining whether there is a loop, and obtaining a loop detection result; S24, obtaining an optimized local map by minimizing the matching residual between the laser radar data frames according to the posture estimation result and the loop detection result; S25, constructing a slam point cloud map based on the key frames in the optimized local map as vertices and the relative postures between the key frames as edges; S26, post-processing the slam point cloud map.

4. The method for detecting faults of unmanned underground mining vehicles according to claim 3, characterized in that: S26, post-processing the SLAM point cloud map, including: Downsample the SLAM point cloud map through the voxel filtering algorithm; The radius filtering algorithm is used to remove outliers from the downsampled slam point cloud map; The ground points are removed from the slam point cloud map after outliers are removed by normal estimation and threshold segmentation methods; The SLAM point cloud map after removing the ground points is subjected to noise filtering through a statistical filtering algorithm to obtain the final SLAM point cloud map.

5. The method for detecting faults of unmanned underground mining vehicles according to claim 1, characterized in that: S3, based on the pre-processed IMU data and vehicle speed data, the odometer data is obtained by fusing them through the Kalman filter algorithm, including: S31, through the laser radar NDT scan matching algorithm, the laser radar data of the current frame is matched with the slam point cloud map, and the initial position of the unmanned vehicle under the slam point cloud map is calculated; S32, solving the preprocessed IMU data to obtain an IMU quaternion reflecting the posture of the unmanned vehicle, wherein the IMU quaternion represents a hypercomplex extension of rotation in three-dimensional space, and includes a real part and three imaginary parts; S33, performing attitude calculation according to the IMU quaternion to obtain the yaw angle yaw of the unmanned vehicle as the heading angle of the unmanned vehicle at the current moment; S34, using the initial posture as a priori estimation, the yaw angle yaw and the preprocessed vehicle speed data as observation quantities, performing data fusion through the Kalman filter algorithm, and estimating the speed v and angular velocity w of the unmanned vehicle at the current moment; S35, based on the estimated velocity v and angular velocity w, calculate the position coordinates of the unmanned vehicle at the current moment through the kinematic model and yaw angle yaw; S36, the current position coordinates of the unmanned vehicle And the yaw angle yaw is used as the odometer data at the current moment.

6. The fault detection method for unmanned underground mining vehicles according to claim 1, characterized in that: S4, through coordinate conversion, converts the pre-processed lidar data, IMU data, vehicle speed data, camera data, and odometer data to the SLAM point cloud map coordinate system, and fuses the converted lidar data, IMU data, vehicle speed data, camera data, and odometer data through the Kalman filter algorithm to obtain the state estimation of the unmanned vehicle, including: S41, converting the pre-processed IMU data, vehicle speed data and odometer data into the SLAM point cloud map coordinate system to obtain the IMU attitude, speed and odometer posture of coordinate system 1; S42, processing the camera data through the ORB-SLAM algorithm to obtain the unmanned vehicle posture in the camera coordinate system, and converting the unmanned vehicle posture in the camera coordinate system to the SLAM point cloud map coordinate system; S43, matching the laser radar data with the SLAM point cloud map by using the laser radar NDT scanning matching algorithm to obtain the unmanned vehicle posture in the laser radar coordinate system, and converting the unmanned vehicle posture in the laser radar coordinate system to the SLAM point cloud map coordinate system; S44, using the odometer posture obtained in S41 as a priori estimation, and establishing the unmanned vehicle state vector and state transfer matrix according to the IMU posture and speed in S41, the unmanned vehicle state vector includes the position , heading angle θ, speed and angular velocity w; S45, taking the IMU attitude of S41 and the angular velocity w of S44 as input, using the state transfer matrix of S44, and through the prediction step of the Kalman filter algorithm, obtains the prior estimation of the state of the unmanned vehicle at the current moment; S46, taking the unmanned vehicle posture in the camera coordinate system of S42, the unmanned vehicle posture in the lidar coordinate system of S43, and the odometer posture of S41 as observation quantities, and correcting the prior estimation of S45 through the updating step of the Kalman filter algorithm to obtain the posterior estimation of the state of the unmanned vehicle at the current moment; S47, using the posterior estimation of S46 as the prior estimation of the Kalman filter algorithm at the next moment, and repeatedly executing S45 and S46 to perform state estimation and data fusion.

7. The method for fault detection of unmanned underground mining vehicles according to claim 1, characterized in that: S5, taking the state estimation of the unmanned vehicle as the predicted value, comparing it with the preprocessed lidar data, IMU data, vehicle speed data and camera data, and calculating the observation residual; Unmanned vehicle fault detection is performed based on the observed residuals, including: S51, using the posterior estimation of the current state of the unmanned vehicle obtained in S46 as the predicted value, and using the laser radar data, IMU data, vehicle speed data or camera data preprocessed in S1 as the observed value z; S52, calculate the residual between the observed value z and the predicted value ; S53, based on the residual And the observation noise covariance matrix R of the corresponding sensor, calculate the residual sum of squares as the innovation amount of the corresponding sensor ; S54, for each sensor, compare the corresponding innovation amount With the preset threshold γ, if is less than or equal to γ, then return to step S51. If If it is greater than γ, it is determined that the corresponding sensor is faulty, and step S55 is executed; S55, obtaining the fault type of the sensor and acquiring a corresponding fault-tolerant strategy.

8. The method for detecting faults of unmanned underground mining vehicles according to claim 7, characterized in that: S55, obtaining the fault type of the sensor and acquiring the corresponding fault tolerance strategy, including: When the faulty sensor is a laser radar, the position and posture of the unmanned vehicle in the laser radar coordinate system in the observation measurement in step S46 is deleted; When the faulty sensor is a camera, the position and posture of the unmanned vehicle in the camera coordinate system in the observation measurement in step S46 is deleted; When the faulty sensor is the IMU, the speed in S44 and the angular velocity w are set to constants; Obtain the fault duration of the sensor and determine whether the fault duration is greater than a threshold. If so, perform data repair.

9. The method for detecting faults of unmanned underground mining vehicles according to claim 8, characterized in that: Obtain the fault duration of the sensor and determine whether the fault duration is greater than a threshold. If so, perform data repair, including: Extract the posterior estimation of the state of the unmanned vehicle at N moments obtained before the sensor failure occurs in step S46 as an interpolation node, and construct an n-order Lagrange interpolation polynomial; Using n-order Lagrange interpolation polynomials, the posterior estimation of the state of the unmanned vehicle at M moments during the fault time period is inferred as virtual observation data during the fault time period and participates in the update step of the Kalman filter algorithm; Calculate the condition number cond(P) of the state estimation covariance matrix of the Kalman filter algorithm; When the condition number cond(P) is greater than the threshold, it indicates that the corresponding filter is damaged, and the user is prompted.

10. A fault detection system for unmanned underground mining vehicles, characterized in that: include: At least one processing unit; used to execute instructions to implement the underground mining unmanned vehicle fault detection method as described in any one of claims 1 to 9.

Images