Cascaded filtering positioning method, equipment and medium in complex and severe environment of well industry and mining

Abstract

The invention discloses a cascading filtering positioning method and device in a complex and severe environment of a well mine and a medium, and achieves a high-precision positioning function of a well mine unmanned vehicle based on a cascading filtering method. According to the method, the IMU, the wheel speed and the wheel rotation angle are subjected to data fusion through adaptive Kalman filtering, the influence of non-Gaussian noise on wheel speed measurement is reduced through non-Gaussian noise feature extraction, and robust prior positioning information is obtained. According to the method, a regular term representing feature degradation is constructed, a laser radar observation model based on a prior map and laser radar data is constructed, and finally, a result of the laser radar observation model and prior positioning information are fused according to the regular term under a regularization Kalman filtering framework to obtain posterior positioning information. The precision and robustness of the method meet the positioning requirements of the mine unmanned vehicle.

Description

TECHNICAL FIELD

[0001] The application relates to the technical field of mine positioning, and in particular to a cascaded filtering positioning method and device under complex and harsh mine environment and a storage medium. BACKGROUND

[0002] Method 1: the current existing similar positioning method based on fusion of IMU, vehicle chassis sensor and laser radar (a fusion positioning method and system for dynamic environment of mine), kinematic estimation is carried out by fusing IMU, vehicle chassis and laser radar data, a dynamic obstacle filtering method is innovatively proposed, finally, the above sensor data is fused based on Kalman filter, a high-precision odometer is constructed for positioning, and the positioning deviation is corrected through the prior map.

[0003] Method 2: the mine environment is a feature degradation environment, and the current existing fusion positioning scheme for feature degradation environment (a multi-source fusion positioning method and system of digital-analog hybrid estimation), which uses IMU data and laser radar data for motion estimation, and proposes a feature degradation evaluation method based on consistency test of adjacent frame point cloud data for calculating uncertainty factor, finally, a dynamic Kalman filter is used to dynamically update the laser radar covariance when fusing IMU and laser radar data according to the feature degradation factor, and when the feature degradation rate higher than the threshold is detected, a pseudo-observation factor is used for observation update.

[0004] Method 3: for feature degradation detection, there is a method based on feature decomposition of observation residual Jacobian matrix (a laser SLAM point cloud registration method based on localizability detection), which is used to estimate the uncertainty of point cloud matching, a feature constraint description matrix is constructed by constructing an observation Jacobian matrix, and a 6-axis (three-axis angle, three-dimensional position) constraint condition is obtained by QR decomposition of the description matrix. And according to the constraint condition, the numerical value of the normal vector corresponding to each point cloud residual is corrected, and the unreliable residual matching result is removed.

[0005] Method 1 has the following defects: Although the method fuses laser radar, IMU and wheel speed data, only the laser radar dynamic interference error is considered when fusing data, and the actual working conditions such as wheel slip caused by wet and slippery mine road are not considered, so that the system is easily disturbed by high-frequency non-Gaussian wheel speed measurement noise; there is no effective detection and system constraint method for feature degradation environment, the stability of laser radar observation model is low in feature degradation environment, and the anti-interference ability is not strong; the dynamic obstacle filtering module needs to traverse two frames of point clouds to calculate the consistency between point clouds, the reliability is low, and the efficiency is not high, which increases the system performance occupation.

[0006] Method 2 has the following defects: The vehicle chassis data cannot be effectively utilized for more accurate positioning, and the pseudo-observation factor is used for low-precision pose estimation after the positioning fails; the method uses inter-frame point clouds for degradation detection, which is inefficient.

[0007] Method 3 has the following defects: 1) the method needs to traverse all point cloud residuals, which is inefficient; The method only involves a single sensor and cannot be used for multi-source sensor data fusion, and the robustness is limited under complex system conditions.

[0008] In summary, the current high-precision positioning related research in mines lacks the ability to resist non-Gaussian measurement noise interference of wheel speed in the actual environment of mines, and the stability of the system observation model is low when the environmental features degrade. The anti-feature degradation scene high-precision positioning related research (a multi-source fusion positioning method and system of digital-analog hybrid estimation) and (a laser SLAM point cloud registration method based on localizability detection) can only guarantee the stability of the positioning system when the features degrade, and cannot guarantee the accuracy of the positioning information. That is, the above-mentioned researches have defects in positioning robustness and positioning accuracy guarantee in feature degradation scenarios, and cannot meet the needs of high-precision and reliable positioning of mine unmanned vehicles for a long time. SUMMARY

[0009] The cascade filtering positioning method, device and storage medium in a complex and harsh environment of a mine are proposed, which can at least solve one of the technical problems in the background art.

[0010] To achieve the above-mentioned purpose, the following technical solutions are adopted in the present application: A cascade filtering positioning method in a complex and harsh environment of a mine, executed by a computer device, includes the following steps, including constructing an anti-slip first filtering module and an anti-feature degradation second filtering module; The anti-slip first filtering module includes a kinematics estimation module, non-Gaussian noise feature extraction, and an anti-slip adaptive Kalman filtering module; The anti-feature degradation second filtering module includes a laser point cloud feature degradation detection module and an anti-feature degradation regularization Kalman filtering module; The kinematics estimation module is used to construct a vehicle kinematics model based on vehicle chassis wheel speed and steering wheel angle information, and to obtain an observation model of IMU kinematics estimation through coordinate conversion according to the IMU and vehicle kinematics external parameters; The non-Gaussian noise feature extraction is used to extract the high-frequency non-Gaussian error features caused by wheel slip, lock and severe jolting of the vehicle chassis wheel speed sensor in the harsh environment of a mine, calculate the wheel speed measurement noise covariance matrix, and provide data basis for subsequent adaptive filtering; An anti-sliding adaptive Kalman filtering module is configured to perform Kalman filtering fusion of vehicle kinematic estimation and IMU measurement according to an estimated adaptive covariance matrix representing non-Gaussian noise of kinematics; A laser point cloud feature degradation detection module is configured to evaluate whether geometric feature constraints for vehicle positioning in real-time scanning point clouds of a laser radar are complete. An anti-feature degradation regularized Kalman filtering module is configured to calculate a Kalman filtering gain according to a feature degradation regularization term, complete anti-feature degradation regularized Kalman secondary filtering, fuse laser radar data and prior positioning information, and obtain a final positioning result.

[0011] In another aspect, the application further discloses a computer readable storage medium storing a computer program, which, when executed by a processor, causes the processor to perform the steps of the above method.

[0012] In still another aspect, the application further discloses a computer device comprising a memory and a processor, wherein the memory stores a computer program, and the computer program, when executed by the processor, causes the processor to perform the steps of the above method.

[0013] According to the above technical solution, the cascade filtering positioning method of the application in the complex and harsh environment of a mine has the following beneficial effects: The application realizes high-precision positioning of an unmanned vehicle in a mine based on a cascade filtering method. The IMU, wheel speed and wheel angle are fused through adaptive Kalman filtering, and the influence of non-Gaussian noise on wheel speed measurement is reduced through non-Gaussian noise feature extraction, so that robust prior positioning information is obtained. The construction of a regularization term representing feature degradation is realized, and a laser radar observation model based on prior map and laser radar data is constructed. Finally, the laser radar observation model result and the prior positioning information are fused according to the above regularization term in the regularization Kalman filtering framework to obtain posterior positioning information, and the precision and robustness meet the positioning requirements of the unmanned vehicle in the mine.

[0014] Specifically, the application relies only on vehicle sensors for positioning and does not depend on external base station positioning information such as UWB, is quick to deploy and has low cost.

[0015] The application positions based on multi-source sensor data and fully utilizes the advantages of multi-source sensors through an efficient data fusion mechanism to ensure that the system can still operate normally in the case of failure of a single sensor, and has high system redundancy.

[0016] The system performs adaptive filtering and data feature analysis in view of the harsh environment of a mine, removes outliers in data fusion, obtains the most accurate fusion positioning estimation, and ensures the accuracy of the positioning system. BRIEF DESCRIPTION OF DRAWINGS

[0017] Figure 1 A flowchart of the present application; Figure 2 A comparative simulation experiment of the anti-skid adaptive filtering of the embodiment of the present application and the traditional Kalman filtering fusion; Figure 3 A comparative diagram of the method of the embodiment of the present application and the traditional Kalman filtering algorithm; Figure 4 A positioning trajectory and a surveying trajectory diagram of the embodiment of the present application. DETAILED DESCRIPTION

[0018] To make the purpose, technical scheme and advantages of the embodiment of the present application clearer, the technical scheme in the embodiment of the present application will be described clearly and completely below with reference to the drawings in the embodiment of the present application. Obviously, the described embodiment is a part of the embodiments of the present application, rather than all the embodiments of the present application.

[0019] As Figure 1 shown, the cascade filtering positioning method in the complex and harsh environment of the well mine described in the embodiment includes an anti-skid first filtering module and an anti-feature degradation second filtering module. The system flowchart is as shown in Figure 1 , the anti-skid first filtering module includes: a kinematics estimation module; a non-Gaussian noise feature extraction; an anti-skid adaptive Kalman filtering module; an anti-feature degradation second filtering module: a laser point cloud feature degradation detection module; an anti-feature degradation regularization Kalman filtering module.

[0020] The kinematics estimation module is used to construct a vehicle kinematics model based on vehicle chassis wheel speed and steering wheel angle information, and to obtain an observation model of IMU kinematics estimation according to the coordinate conversion of IMU and vehicle kinematics external parameters. Wherein, The current vehicle state is defined as

[0021]

[0022] including the positioning attitude , the positioning position , the speed in the world coordinate system , the bias of IMU angular velocity and acceleration and , and the gravity acceleration .

[0023] The error between the true value and the estimated value of the positioning state is , the positioning state estimated value based on IMU integration, and k is the current state sequence number. For generalized subtraction, for n-dimensional manifold and its operation rule is: For ordinary vectors a and b, the operation is: .

[0024] The IMU motion state observation model based on kinematic estimation is:

[0025] Where: is the observation result of vehicle speed, , is the rotation submatrix and displacement submatrix of the external parameter from the kinematic center of the vehicle to the lidar, and are the direction angle matrix and velocity of the lidar in the world coordinate system, is the angular velocity measurement result of the IMU.

[0026] At this time, the observation model is obtained by taking the partial derivative of the error quantity , and the Jacobian matrix of the observation model is obtained:

[0027] Where: The part in which depends on the attitude term is , and corresponds to the rotation error , and the derivative of using the Lie group disturbance model can be obtained:

[0028] Therefore:

[0029] The part in which depends on the velocity term is . Since is an ordinary vector, the partial derivative of is equivalent to the partial derivative of :

[0030] The part in which depends on the IMU angle zero offset is , where , that is, the acceleration measurement value is: sensor measurement value Subtract IMU bias Let Then:

[0031] So,

[0032] Therefore,

[0033] So,

[0034] Where is the skew-symmetric matrix of vector .

[0035] From the above results, we can get: .

[0036] So far, we have obtained the observation model based on vehicle kinematic estimation, which can be used to correct the speed, angle, and IMU angular velocity bias of the IMU measurement results.

[0037] Non-Gaussian noise feature extraction is used to extract the high-frequency non-Gaussian error characteristics of the vehicle chassis speed sensor in the harsh scene of the mine, which are caused by wheel slip, lock and severe jolt. The wheel speed measurement noise covariance matrix is calculated to provide data basis for subsequent adaptive filtering.

[0038] Non-Gaussian error feature extraction model is designed using wheel speed, engine torque, and IMU forward acceleration measurement data. It includes resistance torque consistency representation and acceleration consistency representation: the resistance torque consistency representation is:

[0039] Where

[0040] is the difference between the current speed and the estimated speed under the current torque, N is the sliding window length, is the mean of the speed error in the sliding window.

[0041] And the acceleration consistency representation is:

[0042] Where is the IMU measured acceleration.

[0043] Based on the above features, a comprehensive feature vector is constructed:

[0044] Based on the comprehensive feature vector, a weight matrix is constructed:

[0045] And its positive definiteness guarantee and unit trace constraint.

[0046] Finally, the kinematic estimation adaptive covariance is obtained:

[0047] So far, this module has completed the function of non-Gaussian noise extraction, and constructed the estimated adaptive covariance matrix representing the kinematic non-Gaussian noise, which is used for subsequent vehicle chassis and IMU data fusion.

[0048] The anti-skid adaptive Kalman filter module is mainly used to fuse the vehicle kinematic estimation and IMU measurement according to the estimated adaptive covariance matrix representing the kinematic non-Gaussian noise. This module first calculates the adaptive Kalman filter gain according to the covariance matrix of the IMU integral model and the Jacobian matrix of the vehicle kinematic estimation observation model, and according to the IMU integral covariance , observation model Jacobian matrix and adaptive vehicle kinematic measurement noise covariance

[0049] The calculation method is referred to the paper Solà, Joan. (2015). Quaternion kinematics for the error-state KF. 10.48550 / arXiv.1711.02508.

[0050] According to the positioning state estimation of the IMU integral model and the positioning state estimation of the vehicle kinematic model, as well as the kinematic measurement noise covariance matrix, the positioning state is iteratively updated based on the extended Kalman filter. Finally, the vehicle kinematic estimation and IMU measurement data are fused under the Kalman filter framework, and the anti-skid adaptive Kalman filter prior positioning information is obtained.

[0051] The laser point cloud feature degradation detection module is used to evaluate whether the geometric feature constraints used for vehicle positioning in the real-time scanning point cloud of the laser radar are complete. This module includes using the prior map and laser radar data to construct the laser radar observation model, extracting the nearest neighbor plane features, calculating the residual Jacobian matrix, and finally constructing the feature degradation regularization term.

[0052] ​In the nearest neighbor plane feature extraction module, we combine the prior positioning state information to search the nearest neighbor plane of the current scan point cloud on the prior point cloud map. The points that cannot search the nearest neighbor plane are discarded, which can avoid the interference of non-map dynamic point cloud.

[0053] The residual Jacobian matrix is calculated as: The distance of the current scan point cloud point to the nearest neighbor plane of the map is calculated as the residual value, and the Jacobian matrix corresponding to each point is calculated:

[0054] where is the plane normal vector, is the current pose of the lidar, is the coordinate of the point cloud point in the radar coordinate system, is the external parameter of the IMU and lidar, is the rotation matrix part of the external parameter. The feature degeneration regularization term is constructed as: First, all residual Jacobian matrices are constructed as:

[0055] and QR decomposition is performed on

[0056] Then, the constraint penalty matrix is constructed according to the eigenvalues , to prevent small quantities with denominator 0, where:

[0057] and is the eigenvector corresponding to the eigenvalue, and finally the regularization term is constructed:

[0058] This regularization term is used to limit the point cloud update in the weak constraint direction.

[0059] The anti-feature degeneration regularization Kalman filtering module calculates the Kalman filtering gain according to the feature degeneration regularization term, completes the anti-feature degeneration regularization Kalman second-order filtering, fuses the lidar data and prior positioning information, and obtains the final positioning result.

[0060] The regularization Kalman filtering gain is calculated as:

[0061] where P is the prior positioning covariance, is the lidar observation model Jacobian matrix, and R is the lidar sensor measurement variance.​ is a feature degradation regularization term. Subsequently, the positioning state information is updated using an iterative Kalman filter to obtain anti-feature degradation Kalman filter posterior positioning information.

[0062] The present application uses IMU and vehicle chassis kinematic model for adaptive data fusion, and removes abnormal measurement values through adaptive Kalman filtering to obtain a fused prior vehicle positioning result. At the same time, the matching result of the vehicle-mounted laser radar and the prior point cloud map is used for secondary fusion with the prior positioning, and regularization filtering is performed according to the properties of the laser point cloud to reduce the influence of the missing features of the mine on the positioning reliability, so as to obtain a more accurate posterior positioning result, which is the final positioning result of the system.

[0063] The following is a simulation experiment of the anti-slip adaptive filtering of the present application and the traditional Kalman filter fusion: set the true value of the speed, the IMU measured speed, and the wheel speed measured speed as shown in Figure 2 The green curve is the IMU measurement result, the blue curve is the vehicle chassis speed based on the wheel speed, and the black curve is the true speed. In the red area, simulated wheel speed measurement disturbance is added to simulate the wheel speed measurement error caused by the wheel slip in the mine.

[0064] The method of the embodiment of the present application and the traditional Kalman filter algorithm are compared, Figure 3 The comparison results are as follows: In the slip event, the speed error of the system is reduced by 74.9%, and the active suppression mechanism effectively limits the sudden increase in speed caused by the wheels. Although there is a certain degree of compromise in the non-slip section (root mean square error: 0.0813 m / s → 0.1508 m / s), the overall speed accuracy is still improved by 61.6% (baseline root mean square error 0.4913 m / s → optimized 0.1888 m / s). This design can stabilize the navigation output in the slip event and maintain the positioning accuracy under normal operating conditions.

[0065] The present application algorithm is used for real vehicle testing in the underground roadway of Niantiao Coal Mine, and a 1.5km typical section is selected for experiment. In the section, 3 repeated experiments are conducted underground, and the vehicle is driven along the mapping trajectory as much as possible in each experiment. The positioning trajectory and the mapping trajectory are shown in Figure 4 , and the 3 positioning trajectories are respectively round1, round2, and round3. The error between each positioning trajectory and the mapping trajectory is calculated to obtain the following table:

[0066] ​Where maxAE, MAE, medAE are the maximum error, mean error and median error of the positioning trajectory and the mapping trajectory, respectively, in meters. The 3rd positioning trajectory is basically consistent with the mapping trajectory, and the error mainly comes from the fact that the vehicle fails to accurately travel along the mapping trajectory. The positioning accuracy is high, and the positioning trajectory difference within 1 m can be accurately distinguished.

[0067] In yet another aspect, the present application also discloses a computer readable storage medium, which stores a computer program. The computer program is executed by a processor, so that the processor executes the steps of the above method.

[0068] In yet another aspect, the present application also discloses a computer device, which comprises a memory and a processor. The memory stores a computer program. The computer program is executed by the processor, so that the processor executes the steps of the above method.

[0069] It can be understood that the system, device and storage medium provided by the embodiments of the present application correspond to the method provided by the embodiments of the present application, and the explanation, examples and beneficial effects of the related content can refer to the corresponding part in the above method.

[0070] In the above embodiments, all or part of the embodiments can be realized by software, hardware, firmware or any combination thereof. When realized by software, all or part of the embodiments can be realized in the form of a computer program product. The computer program product comprises one or more computer instructions. When the computer program instructions are loaded and executed on a computer, all or part of the processes or functions described in the embodiments of the present application are generated. The computer can be a general-purpose computer, a special-purpose computer, a computer network or other programmable devices. The computer instructions can be stored in a computer readable storage medium or transmitted from one computer readable storage medium to another computer readable storage medium, for example, the computer instructions can be transmitted from one website, computer, server or data center to another website, computer, server or data center through wired (such as coaxial cable, optical fiber, digital subscriber line (DSL)) or wireless (such as infrared, wireless, microwave, etc.) mode. The computer readable storage medium can be any available medium that can be accessed by a computer or a data storage device such as a server, data center, etc. integrated with one or more available media. The available medium can be a magnetic medium (for example, floppy disk, hard disk, magnetic tape), optical medium (for example, DVD), or semiconductor medium (for example, solid state disk (SSD)) and the like.

[0071] It is to be noted that, in the present text, the relative terms such as first and second, and the like are used merely to differentiate one entity or operation from another entity or operation, without necessarily requiring or implying any such actual relationship or order between such entities or operations. Moreover, the terms "comprising", "containing", or any other variant thereof are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements in the list, but can also include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without more limitations, an element defined by the phrase "comprising a" does not exclude the existence of additional identical elements in the process, method, article, or apparatus that includes the element.

[0072] Each of the embodiments in the present specification is described in a relevant manner, and the same or similar parts between the embodiments can be referred to each other. Each of the embodiments focuses on the difference from other embodiments. In particular, for the system embodiments, since they are basically similar to the method embodiments, the description is relatively simple, and the relevant parts can be referred to the part of the description of the method embodiments.

[0073] The above embodiments are only used to illustrate the technical solutions of the present application, rather than limiting them. Although the present application has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions recorded in the foregoing embodiments, or make equivalent replacements for some technical features. Such modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present application.

Claims

1. A cascaded filtering positioning method in a complex and harsh environment of a mine, characterized in that, The method comprises constructing an anti-sliding first-level filter module and an anti-feature degradation second-level filter module, and is characterized in that The anti-sliding first-level filter module comprises a kinematics estimation module, a non-Gaussian noise feature extraction module and an anti-sliding adaptive Kalman filter module; The anti-feature degradation second-level filter module comprises a laser point cloud feature degradation detection module and an anti-feature degradation regularization Kalman filter module; The kinematics estimation module is configured to construct a vehicle kinematics model based on vehicle chassis wheel speed and steering wheel angle information, and to perform coordinate conversion according to IMU and vehicle kinematics external parameters to obtain an observation model of IMU kinematics estimation; The non-Gaussian noise feature extraction module is configured to extract a high-frequency non-Gaussian error feature caused by wheel skidding, locking and severe jolting of a vehicle chassis wheel speed sensor in a harsh underground mine scene, and to calculate a wheel speed measurement noise covariance matrix to provide a data basis for subsequent adaptive filtering; The anti-sliding adaptive Kalman filter module is configured to perform Kalman filter fusion of vehicle kinematics estimation and IMU measurement according to an estimated adaptive covariance matrix representing non-Gaussian noise of kinematics; The laser point cloud feature degradation detection module is configured to evaluate whether geometric feature constraints for vehicle positioning in real-time scanning point cloud of a laser radar are complete; The anti-feature degradation regularization Kalman filter module is configured to calculate a Kalman filter gain according to a feature degradation regularization term, to complete anti-feature degradation regularization Kalman second-level filtering, to fuse laser radar data and prior positioning information, and to obtain a final positioning result.

2. The cascaded filtering positioning method in a complex and harsh environment of a mine according to claim 1, characterized in that: The kinematics estimation module comprises an IMU motion state observation model based on kinematics estimation, which is as follows: The current vehicle state is defined as including positioning poses , positioning positions , velocities in a world coordinate system , biases of IMU angular velocities and accelerations and , and gravitational acceleration ; is the error of the true value and the estimated value of the positioning state, , is the estimated value of the positioning state based on IMU integration, k is the current state sequence number; is the generalized subtraction, for n-dimensional manifold and its operation rule is: for ordinary vectors a and b, the operation is: ; The IMU motion state observation model based on kinematics estimation is as follows: wherein: is an observation of the vehicle speed, , is a rotation submatrix and a displacement submatrix in the extrinsic parameters of the laser radar from the kinematic center of the vehicle, and are the direction angle matrix and the velocity of the laser radar in the world coordinate system, respectively, is the angular velocity measurement of the IMU; At this time the observation model The error amount The partial derivative is obtained, and the Jacobian matrix of the observation model : Therefore: The partial derivative of the pose-dependent term is while the corresponding rotation error is Using the Lie group perturbation model, we have the derivative of the pose-dependent term with respect to the rotation error is Therefore, The velocity-dependent term is ; since is a covector, taking the partial derivative with respect to is equivalent to taking the partial derivative with respect to : Dependence on IMU angle zero offset Part of Where i.e. acceleration measurement is: sensor measurement minus IMU zero offset Let Then: Therefore, Therefore, Finally, we obtain: wherein is the anti-symmetric matrix of the vector ; The non-Gaussian noise feature extraction specifically comprises 。 3. The cascaded filtering positioning method in complex and harsh environments of underground mines according to claim 2, characterized in that: A non-Gaussian error feature extraction model is designed using wheel speed, engine torque and IMU forward acceleration measurement data, including a resistance torque consistency representation term and an acceleration consistency representation term; The resistance torque consistency representation term is as follows: It is an interpolation of the estimated speed under the current speed and the current torque; wherein; The acceleration consistency representation term is as follows: A comprehensive feature vector is constructed based on the above features: ； A weight matrix is constructed based on the comprehensive feature vector: And its positive definiteness guarantee and unit trace constraint are performed. Finally, the kinematics estimation adaptive covariance is obtained: At this point, this module has completed the function of non-Gaussian noise extraction, and has constructed an estimated adaptive covariance matrix representing non-Gaussian noise of kinematics, which is used for subsequent vehicle chassis and IMU data fusion. According to the positioning state estimation of the IMU integral model, the positioning state estimation of the vehicle kinematics model and the kinematics measurement noise covariance matrix, the positioning state is iteratively updated based on the extended Kalman filter, and finally the vehicle kinematics estimation and IMU measurement data are fused under the Kalman filter framework to obtain anti-sliding adaptive Kalman filter prior positioning information.

4. The cascaded filtering positioning method in complex and harsh environments of underground mines according to claim 3, characterized in that: An anti-slip adaptive Kalman filter module, first according to the covariance matrix of the IMU integral model and the Jacobian matrix of the vehicle kinematics estimation observation model, and according to the IMU integral covariance , the observation model Jacobian matrix and the adaptive vehicle kinematics measurement noise covariance Calculate the adaptive Kalman filter gain: The laser point cloud feature degradation detection module comprises constructing a laser radar observation model using a prior map and laser radar data, extracting nearest neighbor plane features, calculating a residual Jacobian matrix and finally constructing a feature degradation regularization term; 5. The cascaded filtering positioning method in complex and harsh environments of underground mines according to claim 4, characterized in that: ​ The nearest neighbor plane extraction module combines prior localization state information to search for a nearest neighbor plane of the current scan point cloud on a prior point cloud map, and points for which a nearest neighbor plane cannot be searched are discarded; wherein the residual Jacobian matrix is calculated as: The distance of the current scan point cloud to the nearest neighbor plane of the map is calculated as a residual value, and the Jacobian matrix corresponding to each point is calculated: wherein is a plane normal vector; wherein the feature degradation regularization term is constructed by first constructing all residual Jacobian matrices as: And to perform QR decomposition: Then a constraint penalty matrix is constructed according to the eigenvalues wherein: Finally, the regularization term is constructed as: The regularization term is used to limit point cloud updates in weak constraint directions.

6. The cascaded filtering positioning method in complex and harsh environments of underground mines according to claim 5, characterized in that: The regularization Kalman filter gain in the anti-feature degradation regularization Kalman filter module is calculated as: where P is the prior positioning covariance, is the laser radar observation model Jacobian matrix, and R is the laser radar sensor measurement variance, is the feature degradation regularization term; the positioning state information is updated using the iterative Kalman filter, and the anti-feature degradation Kalman filter posterior positioning information is obtained.

7. A computer readable storage medium storing a computer program, characterized in that, The computer program, when executed by a processor, causes the processor to perform the steps of the method of any one of claims 1 to 6.

8. A computer device comprising a memory and a processor, the memory storing a computer program, characterized in that, The computer program, when executed by the processor, causes the processor to perform the steps of the method of any one of claims 1 to 6.

Images