A tightly coupled adaptive filtering localization method for UAVs in coal mine environments

Abstract

This invention relates to the field of coal mining technology, specifically to a tightly coupled adaptive filtering localization method for UAVs in coal mine environments. The method includes the following steps: acquiring and preprocessing lidar data, inertial navigation data, and environmental data for the current filtering cycle; obtaining the initial state value and its covariance matrix from the inertial navigation data for the current filtering cycle, and performing forward propagation based on disturbance intensity and dust concentration; performing intra-frame motion compensation on the point cloud positions in the lidar data based on the prior state to obtain a weighted, distortion-free point cloud set; removing dynamic point clouds and constructing an observation set by combining the weighted, distortion-free point cloud set; calculating the Kalman gain matrix and correcting the state and covariance, then applying environmental constraints after correction to obtain the posterior state vector and posterior covariance matrix; repeating the above steps in the next filtering cycle to achieve continuous localization of the UAV. This invention can enhance the robustness of UAV localization in complex coal mine environments.

Description

Technical Field

[0001] This invention relates to the field of coal mining technology, specifically to a tightly coupled adaptive filtering positioning method for UAVs in coal mining environments, which achieves UAV positioning through tight coupling of lidar and inertial navigation and adaptive filtering. Background Technology

[0002] With the continuous expansion of the application scope of unmanned systems, the fusion positioning technology of lidar and inertial measurement unit (IMU) has become an important means to achieve high-precision autonomous navigation. However, in harsh environments such as high dust concentration, strong airflow disturbance, and insufficient lighting, sensor observations are easily interfered with, resulting in a significant decrease in the stability and accuracy of filtered estimation.

[0003] Existing lidar-inertial integrated navigation methods typically rely on extended Kalman filtering (EKF) or iterative extended Kalman filtering (iEKF) frameworks for state estimation. However, most methods assume stable environmental noise statistics and fail to adequately account for changes in observation and prediction errors caused by non-ideal factors such as coal dust obstruction, wind disturbance, and dynamic obstacles. Furthermore, traditional point cloud distortion correction and feature matching methods are often based on fixed noise models, making it difficult to maintain consistency under conditions of fluctuating wind speed or varying dust concentrations. Due to the time-varying and uncertain nature of environmental disturbances in the complex environment of coal mines, existing positioning algorithms are prone to attitude drift and positioning instability.

[0004] Therefore, there is an urgent need for a radar-inertial fusion positioning method that can dynamically adjust filtering parameters and enhance robustness in complex environments to improve the stable flight and reliable navigation capabilities of coal mine UAVs under multiple interference conditions. Summary of the Invention

[0005] To address the issue of decreased positioning accuracy and stability of unmanned aerial vehicles (UAVs) in underground coal mine environments, this invention proposes a tightly coupled adaptive filtering positioning method for UAVs in coal mine environments. By fusing coal mine environment dust concentration data, disturbance intensity data, and dynamic obstacle data, the method achieves dynamic coupling between environmental perception and estimation processes, thereby improving the robustness of UAV positioning.

[0006] To solve the above-mentioned technical problems, the technical solution adopted by the present invention is: a tightly coupled adaptive filtering positioning method for UAVs in coal mine environments, comprising the following steps: Step 1: Acquire the lidar data and inertial navigation data for the current filtering cycle, and perform coordinate transformation and time synchronization processing to obtain a standardized data packet; at the same time, acquire environmental data, including dust concentration data, disturbance intensity data, and dynamic obstacle tags; Step 2: Obtain the initial state value and its covariance matrix of the current filtering cycle from the inertial navigation data. Perform forward propagation based on the disturbance intensity and dust concentration to obtain the prior state containing attitude, velocity and position. At the same time, construct the process noise covariance matrix based on the disturbance intensity and dust concentration, and perform forward propagation through the process noise covariance matrix to obtain the prior covariance matrix. Step 3: Based on the prior state, perform intra-frame motion compensation on the point cloud set in the lidar data to obtain the coordinate correction value of each point cloud in the reference time coordinate system, and calculate the point-level confidence weight coefficient to obtain the weighted distortion-free point cloud set. Step 4: Remove point clouds labeled as dynamic obstacles, use the remaining point clouds as observation points, and calculate the normal geometric residual, observation Jacobian matrix and single-point measurement noise covariance of each observation point by combining the coordinate correction value and point-level confidence weight coefficient in the weighted distortion-free point cloud set, and construct the observation set. Step 5: Using the observation set and the prior state and prior covariance matrix obtained from forward propagation as input, calculate the Kalman gain matrix and correct the state and covariance. After correction, apply environmental constraints to the posterior covariance matrix to finally obtain the posterior state vector and posterior covariance matrix of the fused observations. Step 6: Repeat steps 1-5 in the next filtering cycle to achieve continuous positioning of the UAV.

[0007] In step 1, the specific method for coordinate transformation and time synchronization of lidar data and inertial navigation data is as follows: When timing is assigned to LiDAR data and inertial navigation data under a unified clock, if the time difference between a single frame of LiDAR data and adjacent inertial navigation data is greater than a threshold, linear interpolation resampling is performed on the inertial navigation data so that each frame of LiDAR data obtains the attitude and velocity at the corresponding moment, and the scanning span and point-level timestamp of each frame of data are recorded. Then, the point cloud of the LiDAR data is converted to the inertial navigation coordinate system using the extrinsic parameter matrix obtained through offline calibration.

[0008] In step 1, the specific method for obtaining dust concentration data, disturbance intensity data, and dynamic obstacle tags is as follows: Obtain the dust concentration data output by the dust sensor, and perform nearest neighbor or linear interpolation according to the timestamp to obtain the dust concentration data corresponding to the point-level timestamp of each frame of data; The disturbance intensity is calculated based on the residual statistics of the previous filtering cycle; Determine if there are two frames of point cloud in the LiDAR data whose velocity exceeds the threshold. If so, mark them as dynamic obstacles.

[0009] In step 2, the formula for calculating the prior state is: ; in, This represents the prior state of the current filtering cycle. This represents the posterior state of the previous filtering cycle. This represents the nonlinear function corresponding to the state equation, where This represents the state corresponding to time t; This represents the inertial navigation data corresponding to time t; The process noise representing the external disturbance is combined with the disturbance intensity during the integration process. Adaptive adjustment of the integral step size; The motion state equation of the UAV is expressed as: ; The derivative of the position p of the UAV in the global coordinate system is given by: Represents the velocity vector The derivative of Indicates acceleration. This represents the attitude rotation matrix from the machine coordinate system to the global coordinate system. express The derivative of and These represent the bias terms for the accelerometer and gyroscope, respectively. This indicates the noise measured by the accelerometer. Represents the gravitational acceleration vector; Indicates angular velocity, Indicates gyroscope noise. Represents the mapping operation for antisymmetric matrices; The formulas for calculating the process noise covariance matrix and the prior covariance matrix are as follows: ; in, This represents the process noise covariance matrix corresponding to the previous filtering cycle. Indicates the baseline covariance. , This represents the adjustment coefficient. This represents the average value of the dust sensor readings over the previous filtering cycle. This represents the trace of the disturbance intensity during the previous filtering cycle. This represents the prior covariance of the current filtering cycle. and These represent the state transition matrix and noise transfer matrix corresponding to the previous filtering cycle, respectively. This represents the posterior covariance matrix corresponding to the previous filtering cycle. This represents the transpose of a matrix.

[0010] In step 3, the weighted distortion-free point cloud set is represented as: ; Among them are: ; ; This represents a weighted set of distortion-free point clouds. Indicates the first The coordinate correction values ​​of a point cloud in the reference time coordinate system after motion compensation. Represents the confidence weight coefficient of the i-th point cloud; and Representing reference time and sampling time The attitude rotation matrix corresponding to the body coordinate system to the world coordinate system. Indicates the first The original coordinate measurements of the point cloud in the lidar coordinate system and They represent the sampling times respectively. and reference time The position vector of the origin of the machine system in the world coordinate system; This represents the preset dust concentration attenuation coefficient. Indicates the sampling time Dust concentration.

[0011] In step 4, the normal geometric residual Observation of the Jacobian matrix and single-point measurement noise covariance The calculation formula is: ; = ; ; in, , , Let represent the normal geometric residual, observation Jacobian matrix, and single-point measurement noise covariance corresponding to the i-th observation point, respectively; This represents the local normal vector corresponding to the i-th observation point. transpose, This represents the coordinate correction value of the i-th point cloud in the reference time coordinate system after motion compensation. express Orthogonal projection points in a local map This represents the observation derivative information corresponding to the i-th observation point. and Let represent the attitude rotation matrix of the body coordinate system relative to the world coordinate system at the reference time, and the position vector of the origin of the body coordinate system in the world coordinate system, respectively. This represents the reference noise under sensor calibration. This represents the confidence weight coefficient of the i-th point cloud. This is a preset positive small constant used to avoid the denominator being zero.

[0012] In step 5, the environmental constraints applied to the posterior covariance matrix are as follows: ; in, and These represent the posterior covariance matrices before and after applying environmental constraints in the current filtering cycle, respectively. , This represents the adjustment coefficient. This indicates the disturbance intensity in the current filtering period. This represents the average value of the dust sensor readings within the current filtering cycle. It represents the identity matrix of the same order as the posterior covariance matrix.

[0013] In step 5, the formula for calculating the Kalman gain matrix and performing state and covariance correction is as follows: ; in, , Let these represent the prior state vector and prior covariance matrix before observation fusion in the current filtering cycle, respectively. , This represents the posterior state vector and posterior covariance matrix after fusion of observations. This is the observation residual vector constructed from all observations in this period. To obtain the observed Jacobian matrix linearized at the prior state, This is the corresponding measurement noise covariance matrix. Here is the Kalman gain matrix. It is the identity matrix. Indicates matrix transpose operation, subscript Used to indicate the current filtering period.

[0014] The aforementioned tightly coupled adaptive filtering localization method for UAVs in coal mine environments further includes the following steps: Step 7: Optimize decision-making based on posterior state variables and posterior covariance matrix to achieve UAV control.

[0015] In step 7, decision optimization is performed based on near-end strategy optimization.

[0016] Compared with the prior art, the present invention has the following advantages: This invention provides a tightly coupled adaptive filtering positioning method for UAVs in coal mine environments. By constructing three types of environmental characterization quantities—dust concentration field, wind disturbance intensity field, and dynamic obstacle field—and mapping them to the process noise and observation noise of the Kalman filter, the Kalman filter is adaptively adjusted to environmental uncertainties, thereby enhancing the robustness of UAV positioning in complex coal mine environments. Attached Figure Description

[0017] Figure 1 A flowchart illustrating a tightly coupled adaptive filtering positioning method for UAVs in a coal mine environment, provided by an embodiment of the present invention; Figure 2 This is a schematic diagram of coal mine environment modeling in an embodiment of the present invention; Figure 3 This is a schematic diagram showing the comparison of positioning accuracy under different working conditions in an embodiment of the present invention; Explanation of the attached diagram labels: 1 is lidar, 2 is IMU inertial unit, 3 is dust concentration sensor, and 4 is simulated tunnel. Detailed Implementation

[0018] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below. Obviously, the described embodiments are some embodiments of the present invention, but not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0019] like Figure 1 As shown, this embodiment of the invention provides a tightly coupled adaptive filtering localization method for UAVs in coal mine environments, including the following steps: Step 1: Acquire the lidar data and inertial navigation data for the current filtering cycle, and perform coordinate transformation and time synchronization processing to obtain a standardized data packet; at the same time, acquire environmental data, including dust concentration data, disturbance intensity data, and dynamic obstacle tags.

[0020] In this embodiment, as Figure 2 As shown, the drone platform includes a LiDAR (Light Detection and Ranging) unit 1, an IMU (Inertial Measurement Unit) unit 2, and a dust concentration sensor 3. The LiDAR unit 1 is used to output point cloud data, the IMU unit 2 is used to output IMU sequences, specifically including angular velocity and acceleration sequences, and the dust concentration sensor 3 is used to output the dust concentration measurement value at the current position of the drone.

[0021] In a coal mine environment, the high-speed movement of the UAV and the difference in sampling frequencies between LiDAR 1 and IMU inertial unit 2 can easily lead to temporal misalignment of point cloud data and angular velocity and acceleration sequence data, thus affecting the accuracy of fusion positioning. To ensure that prediction, distortion correction, and filtering updates operate under a unified reference frame and a unified time base, this embodiment sets up a data input and time synchronization module to standardize and integrate LiDAR data, inertial navigation data, and environmental quantities. First, the IMU and LiDAR data are timed under a unified clock.

[0022] Specifically, in step 1, the method for coordinate transformation and time synchronization of LiDAR data and inertial navigation data is as follows: The LiDAR data and inertial navigation data are timed under a unified clock. If the time difference between a single frame of LiDAR data and adjacent inertial navigation data is greater than a threshold (e.g., 5ms), linear interpolation resampling is performed on the inertial navigation data so that each frame of LiDAR data obtains the attitude and velocity at the corresponding moment, and the scanning span and point-level timestamp of each frame are recorded. Then, the point cloud of the LiDAR data is transformed to the inertial navigation coordinate system using the extrinsic parameter matrix obtained through offline calibration. The inertial navigation coordinate system is aligned with the global coordinate system during the initialization phase, thereby enabling the LiDAR data and inertial navigation data to achieve a unified expression in the global coordinate system.

[0023] The lidar data and inertial navigation data use a unified time reference, with alignment errors controlled within 1ms. External parameters are provided by offline calibration, and their uncertainties are recorded: attitude error not exceeding 0.2° and translation error not exceeding 5mm. These uncertainties are considered during initialization to avoid overconfidence.

[0024] Specifically, in step 1, the method for obtaining environmental dust concentration data, disturbance intensity data, and dynamic obstacle tags is as follows: Obtain the dust concentration data output by the dust sensor, and perform nearest neighbor or linear interpolation according to the timestamp to obtain the dust concentration data corresponding to the point-level timestamp of each frame of data; The disturbance intensity is calculated based on the residual statistics of the previous filtering cycle; Determine if there are two frames of point cloud in the LiDAR data whose velocity exceeds the threshold. If so, mark them as dynamic obstacles.

[0025] After the above synchronization and environmental data acquisition are completed, the disturbance intensity, dust concentration data at each time point, and dynamic obstacle labels are cached as environmental attributes. The disturbance intensity does not require direct measurement of ambient wind speed using a dedicated sensor; it is obtained online by statistically analyzing the residual from the previous filtering cycle within a sliding time window and transmitted as an adaptive input to the prediction side with the standardized data packet in the current cycle. In this embodiment, the disturbance intensity is used to characterize the uncertainty in the prediction process caused by wind disturbance in the coal mine environment and other unmodeled dynamic factors, serving as one of the input bases for the subsequent adaptive construction of the process noise covariance.

[0026] In coal mining environments, dynamic obstacles (such as falling rocks and moving vehicles) frequently appear in the field of view of LiDAR (Light Detection and Ranging) systems. Due to the rapid changes in these obstacles, the system may misidentify them as static features, thus affecting positioning accuracy and map building. To address this issue, effective modeling of dynamic obstacles is essential.

[0027] The process of identifying dynamic obstacles includes: First, by comparing point cloud data across consecutive frames, points with significant displacement are detected. These points are considered potential dynamic obstacles. Then, the system analyzes the displacement, velocity, and other characteristics of the point cloud. If the movement speed of the point cloud exceeds a preset threshold, it is marked as a dynamic obstacle. The marked dynamic point cloud undergoes special processing during filtering and localization, typically by weighting to reduce its influence or by directly removing it, to prevent it from interfering with the matching of the static map.

[0028] In this way, the system can effectively distinguish between dynamic obstacles and static environmental features, thereby ensuring the stability of the filtering process, improving positioning accuracy, and enhancing the obstacle avoidance capability of UAVs in complex coal mine environments. If the estimated velocity of a point cloud exceeds a threshold for two consecutive frames in adjacent frames, it is marked as a dynamic point; marked points are removed when constructing the observation set. Define dynamic obstacle labels: (1) Dynamic obstacle labels are used in the subsequent filtering update stage for observation set processing: when When the point cloud is removed from the observation set during the construction of the normal geometric residual, its weight in the Kalman gain approaches zero, so as to avoid the dynamic target being misused as a static constraint and introducing positioning error.

[0029] Step 2: Obtain the initial state value and its covariance matrix of the current filtering cycle from the inertial navigation data. Perform forward propagation based on the disturbance intensity and dust concentration to obtain the prior state containing attitude, velocity and position. At the same time, construct the process noise covariance matrix based on the disturbance intensity and dust concentration, and propagate through the process noise covariance matrix to obtain the prior covariance matrix.

[0030] In step 2, the motion state of the UAV is continuously predicted in a unified coordinate reference system using time-aligned inertial navigation data as input. The initial input for the prediction is the initial state value x and its initial covariance matrix for the current filtering cycle, where... , This represents the position vector of the UAV in the global coordinate system. Represents the velocity vector. This represents the attitude rotation matrix from the body coordinate system to the world coordinate system. This indicates the accelerometer bias term. This represents the gyroscope bias term. The initial state value x and its covariance matrix can be derived from the set values ​​during system initialization or the filtering update results of the previous cycle. By combining the acceleration and angular velocity sequences and extrinsic parameter matrices in the inertial navigation data, the prediction of the prior state and the prior covariance can be achieved, providing dynamic input for point cloud distortion correction and filtering updates in subsequent lidar data.

[0031] In this embodiment, the continuous-time motion model of the UAV consists of three parts: position, velocity, and attitude, and its motion state equation is expressed as: (2) in, The derivative of the position p of the UAV in the global coordinate system is given by: Represents the velocity vector The derivative of Indicates acceleration. This represents the attitude rotation matrix from the machine coordinate system to the global coordinate system. express The derivative of and These represent the bias terms for the accelerometer and gyroscope, respectively. This represents the noise (random disturbance) in the accelerometer measurement. Represents the gravitational acceleration vector; Indicates angular velocity, Indicates gyroscope noise. This represents the mapping operation on antisymmetric matrices.

[0032] The above equation yields the change of the attitude matrix over continuous time through integration. To discretize the above continuous-time state equation to a filter period... In this embodiment, a fourth-order Runge-Kutta numerical integral is used to predict the prior state. Represented as: (3) in, This represents the prior state of the current filtering cycle. This represents the posterior state of the previous filtering cycle, which is used as the initial prediction value for the current filtering cycle. This represents the nonlinear function corresponding to the state equation, where This represents the state corresponding to time t; This represents the inertial navigation data corresponding to time t; The process noise represents the external disturbance; the disturbance intensity is incorporated during the integration process. Adaptively adjust the integration step size; when the trace of the disturbance intensity Increasing the step size suppresses integration error, ensuring stable convergence of the integral in areas of severe wind disturbance and improving computational efficiency in areas of stable environment. Specifically, the formula for calculating the integration step size is as follows: ,in, Indicates the current integration step size. The preset reference integration step size is determined by the system sampling frequency. The preset positive adjustment coefficient is determined by offline experimental calibration.

[0033] Specifically, since changes in wind speed and fluctuations in coal dust concentration increase the uncertainty of the system dynamics model, after completing the prior state prediction, this invention adaptively constructs the process noise covariance matrix based on the environmental parameters of the previous filtering cycle, and then propagates the prior covariance matrix of the current filtering cycle from the process noise covariance matrix. Specifically, the calculation formulas for the process noise covariance matrix and the prior covariance matrix are as follows: (4) in, This represents the process noise covariance matrix of the previous filtering cycle. Indicates the baseline covariance. , This represents the adjustment coefficient. This represents the average value of the dust sensor readings over the previous filtering cycle. Indicates the disturbance intensity obtained in the previous filtering cycle. traces, This represents the prior covariance of the current filtering cycle. and These represent the state transition matrix and noise transfer matrix corresponding to the previous filtering cycle, respectively. This represents the posterior covariance matrix corresponding to the previous filtering cycle. This represents the transpose of the matrix. This is achieved by analyzing the process noise covariance matrix. Adaptive adjustments can be made to increase the intensity of disturbances caused by drastic local wind speed fluctuations. The corresponding components; when the residual statistics indicate that the environmental wind field tends to stabilize, reduce This enhances the reliability of IMU predictions and ensures dynamic constraints on the state variance of the Kalman filter under wind disturbance conditions. The subscripts k and k+1 in the formula represent the previous and current filtering cycles, respectively.

[0034] In step 2, a priori states including attitude, velocity, and position can be obtained through the forward propagation prediction process. and its covariance This serves as the input for the subsequent integration and observation residual construction in the lidar motion compensation stage, providing a time-continuous dynamic prior basis for the entire filtering loop. It should be noted that point-level dust concentration and dynamic obstacle labels do not directly participate in the forward propagation prediction equation. The point-level dust concentration is used to subsequently calculate the point-level confidence weight, and the influence of the observation point on the filter update is adjusted through the single-point measurement noise covariance in step 4. Dynamic obstacle labels can be used to remove dynamic point clouds before constructing the observation set.

[0035] Step 3: Based on the prior state, perform intra-frame motion compensation on the point cloud set in the lidar data to obtain the coordinate correction value of each point cloud in the reference time coordinate system, and calculate the point-level confidence weight coefficient to obtain the weighted distortion-free point cloud set. .

[0036] In the forward propagation phase of step 2, the prior state of the UAV in the current filtering cycle is obtained. with prior covariance However, the lidar completes the measurement point by point within a single frame scan cycle, during which the UAV's attitude and position continuously change. Ignoring this motion will lead to spatial distortion of the point cloud. Therefore, in step 3 of this paper, a scanning reference time is used. Treat Using the same continuous dynamic equations and fourth-order Runge-Kutta numerical integration as in forward propagation, the state is advanced at high frequency only within this frame scan window. The integration input is time-aligned inertial navigation data. To ensure numerical stability and efficiency, the integration step size is determined based on the disturbance intensity. Adaptive adjustment: shorten the step size when wind disturbance intensifies and widen the step size when the environment is stable; orthogonalize the attitude rotation matrix after each update to maintain the consistency of the attitude matrix. The pose at any sampling moment of the scanning window of this frame can be obtained by integration, where the pose includes the attitude rotation matrix from the body coordinate system to the world coordinate system at the sampling moment and the position vector of the origin of the body coordinate system at the sampling moment in the global coordinate system, that is, the position of the UAV at that moment.

[0037] Select reference time Based on this, all sampling points are unified to the reference time coordinate system, and the coordinates of the motion-compensated point cloud can be expressed as: (5) Considering the impact of coal dust and dynamic targets on observation reliability, point-level confidence weight coefficients are assigned to the distortion-free point cloud. The formula for calculating the confidence weight coefficients is as follows: (6) in, Indicates the first The original coordinate measurements of the point cloud in the lidar coordinate system Indicates the number after exercise compensation The coordinate correction values ​​of each point cloud in the reference time coordinate system; and Representing reference time and sampling time The attitude rotation matrix corresponding to the body coordinate system to the world coordinate system. and They represent the sampling times respectively. and reference time The position vector of the origin of the machine system in the world coordinate system; This represents the confidence weight coefficient of the i-th point cloud. This represents the preset dust concentration attenuation coefficient, used to control the attenuation intensity of dust concentration on the confidence weight of the point cloud, and is determined by offline experimental calibration. Indicates the sampling time Dust concentration.

[0038] In step 3, the weighted distortion-free point cloud set Represented as: (7) The motion compensation and weighted distortion correction transformation described above achieve temporal unification of the point cloud, eliminating spatial misalignment caused by pose changes within the frame. This results in a weighted distortion correction point cloud set. The derivative information of the observation model is extracted along the same integral trajectory, thus ensuring that distortion correction and linearization are performed under consistent motion assumptions. Therefore, this step obtains the pose at the reference time through motion compensation, including the reference time... Attitude rotation matrix from body coordinate system to world coordinate system Reference time The position vector of the origin of the machine system in the world coordinate system and the weighted, distortion-free point cloud set used to construct the observations This provides information for subsequent observations of derivatives. The determination of the necessary inputs completes the smooth transition from dynamic prediction to observation constraints. In this step, intra-frame motion compensation corrects the geometric position of the point cloud based solely on the prior state trajectory from step 2, without altering point-level environmental attributes; dust concentration Dynamic obstacle label and the point-level confidence weight coefficients calculated from dust concentration. The point cloud, after intra-frame motion compensation, is then transferred to the next observation construction stage.

[0039] Step 4: Remove point clouds labeled as "dynamic obstacle". Use the remaining point clouds as observation points and calculate the normal geometric residual for each observation point by combining the coordinate correction values ​​and point-level confidence weight coefficients from the weighted distortion-free point cloud set. Observation of the Jacobian matrix and single-point measurement noise covariance , construct an observation set.

[0040] In step 4, based on the motion compensation performed in step 3, observation modeling and linearization are completed, and the results are organized into an observation set that can be directly entered into the next step of filtering and updating. Under a unified coordinate system, the coordinate correction values ​​for each motion-compensated element in the local map are... The point cloud is used to retrieve its nearest neighbor in the local point cloud map through a KD-tree-based spatial index. The neighborhood point set is then fitted with a least-squares plane to obtain the local normal vector corresponding to the fitted plane. Then adjust the coordinate values. When orthogonally projected onto this plane, the projection point is denoted as... . and All with In the same coordinate system.

[0041] In step 4, the normal geometric residual Observation of the Jacobian matrix and single-point measurement noise covariance The calculation formula is: ; (8) = (9) ; (10) in, , , Let represent the normal geometric residual, observation Jacobian matrix, and single-point measurement noise covariance corresponding to the i-th observation point, respectively; This represents the local normal vector corresponding to the i-th observation point. transpose, This represents the coordinate correction value of the i-th point cloud in the reference time coordinate system after motion compensation. express Orthogonal projection points in a local map This represents the observation derivative information corresponding to the i-th observation point, using the coordinate correction value and pose (including the attitude rotation matrix representing the body coordinate system relative to the world coordinate system at the reference time) at the reference time. and the position vector of the origin of the body coordinate system in the world coordinate system. Therefore, in this invention, point cloud observation is highly sensitive to attitude changes (rotation part) and position changes (translation part). This represents the reference noise under sensor calibration. This represents the confidence weight coefficient of the i-th point cloud. This is a preset positive small constant used to avoid the denominator being zero. i represents the index of the i-th observation point (consistent with the corresponding point cloud sequence number).

[0042] Among them, normal geometric residual The results rely on the time unification of the lidar motion compensation phase, thus reflecting the geometric deviation caused by pose estimation errors after time unification, rather than intra-frame distortion caused by pose changes during scanning. (Observation derivative information) The noise covariance of a single-point measurement is calculated using the motion compensation output of the lidar. The confidence weight coefficient is adjusted by the point-level confidence weight coefficient; this coefficient changes as dust concentration increases or the confidence weight of the observation point decreases. Reduce, thus reducing the corresponding single-point measurement noise covariance Increasing the concentration reduces the impact of that observation point on the Kalman filter update. It should be noted that the dust concentration only affects the adjustment of the single-point measurement noise covariance through the point-level confidence weight coefficient, without changing the geometric expression of the residuals, thus ensuring the stability of the filter structure. For The point cloud, specifically those identified as dynamic obstacles, is removed before constructing the observation set and is not included in the construction of the normal geometric residual, observation Jacobian matrix, and single-point measurement noise covariance. The observation set can then be constructed using the above formula, and subsequently used as input for the update step in the next Kalman filtering process.

[0043] Step 5: Using the observation set and the prior state and prior covariance matrix obtained from forward propagation as input, calculate the Kalman gain matrix and correct the state and covariance. After correction, apply environmental constraints to the posterior covariance matrix to finally obtain the posterior state vector and posterior covariance matrix after fusion of observations.

[0044] In step 5, the formula for calculating the Kalman gain matrix and performing state and covariance correction is as follows: (11) in, , Let these represent the prior state vector and prior covariance matrix of the current filtering cycle, respectively. , This represents the posterior state vector and posterior covariance matrix after fusion of observations. The observed residual vector is constructed from all normal geometric residuals of the current filtering period. The observed Jacobian matrix is ​​represented by the observed derivative information of each observation point in step 4. constitute, It is the reference pose output in step 3, the motion compensation stage (including) and Linearization is obtained at (). This represents the measurement noise covariance matrix, which is the single-point measurement noise covariance. The diagonal matrix formed; Represents the Kalman gain matrix. Represents the identity matrix. Indicates matrix transpose operation, subscript This indicates the current filtering period.

[0045] To improve convergence stability under strongly perturbed scenarios, environmental constraints can be applied to the covariance after the update to avoid overly optimistic estimation. Specifically, in step 5, the environmental constraints applied to the posterior covariance matrix are as follows: (12) in, and Let these represent the posterior covariance matrices before and after applying environmental constraints, respectively. , This represents the adjustment coefficient. This indicates the disturbance intensity obtained within the current filtering period. This represents the average value of the dust sensor readings within the current filtering cycle. This represents the identity matrix of the same order as the posterior covariance matrix. This constraint moderately increases posterior uncertainty in areas with severe wind disturbance or significant dust, and, together with the adaptive mechanism of the observation noise covariance, enhances the robustness of the filtering update process to environmental changes.

[0046] Following the standard Kalman filter (iEKF) procedure, the updated posterior state vector is used. With the posterior covariance matrix The observed residual vector r and the observed Jacobian matrix H can be relinearized, and state updates can be iteratively performed until convergence. This embodiment sets an upper limit on the number of iterations to ensure numerical stability. The final output posterior state vector... and the posterior covariance matrix after applying environmental constraints These parameters serve as the positioning estimation result and uncertainty characterization quantity for the current cycle, respectively, and as the initial input for forward propagation in the next filtering cycle, forming a closed-loop filtering structure of "prediction-observation-update". By introducing dust concentration, disturbance intensity, and dynamic obstacle information, the process noise on the prediction side and the measurement noise on the observation side are adaptively adjusted, which can suppress the influence of unreliable observations in complex coal mine environments and improve the robustness and stability of positioning estimation.

[0047] Step 6: Repeat steps 1-5 in the next filtering cycle to achieve continuous positioning of the UAV.

[0048] Based on the filtering update process in steps 1-5 above, this invention outputs a posterior state estimate in each filtering cycle. and its covariance The posterior state estimate, as the state estimation result, can be used as input to the UAV state perception module to provide pose and velocity information for subsequent motion control, path planning, or mission decision-making. The corresponding posterior covariance is used to characterize the uncertainty level of the positioning result and can be used to constrain control commands or trigger conservative control strategies to improve the system's operational safety and stability in complex environments. The specific usage of the above outputs can be configured according to the application scenario and does not constitute an essential technical feature of the positioning method of this invention.

[0049] Furthermore, the tightly coupled adaptive filtering localization method for UAVs in a coal mine environment according to this embodiment also includes the following steps: Step 7: Optimize decision-making based on posterior state variables and posterior covariance matrix to achieve UAV control.

[0050] Specifically, in step 7, decision optimization is performed based on Proximity Policy Optimization (PPO).

[0051] In this embodiment, the positioning method of the present invention was verified by establishing multiple disturbance models in a simulation environment, including a wind disturbance model, a coal dust diffusion model, and a dynamic obstacle model.

[0052] In the wind disturbance model, the three-dimensional wind speed vector and wind speed variance matrix are respectively represented as: (13) (14) in, Indicates the location The three-dimensional wind speed vector at time t, including wind speeds in the x, y, and z directions. , The reference wind speed vector, which varies with time, is given by a preset wind field time series function in the simulation environment; and These represent the current spatial coordinates and the coordinates of the wind field center, respectively. The spatial attenuation coefficient of wind speed controls the rate attenuation of wind speed with distance from the center of the wind field and is preset by the simulation environment. Indicates wind speed The variance matrix, where The wind field model is used only to construct multi-condition disturbance scenarios during the simulation phase, and the parameters are preset by the simulation environment. In the simulation, wind speed disturbance conditions are generated by formula (13) to construct controllable and repeatable strong wind disturbance conditions to test the robustness of the algorithm; the wind field model is only used as a disturbance generation method for the simulation environment and does not participate in the state estimation of the positioning algorithm of this invention. In actual deployment, the wind disturbance influence is obtained by online estimation of the disturbance intensity through the filtered residual sequence. Equivalent compensation is performed. The disturbance intensity data is obtained from the statistical analysis of the observation residuals of the previous filtering cycle and is used to characterize the prediction uncertainty caused by wind disturbance, airflow disturbance, and unmodeled dynamic factors.

[0053] The coal dust diffusion model is as follows: (15) in, This represents the coal dust concentration at a spatial point. Indicates the intensity of the emission source. Indicates the location of the particle source center. The diffusion radius of the particle source as a function of time is given by: (16) The initial diffusion radius of the particle source is preset by the simulation environment; This represents the diffusion rate coefficient, which controls how quickly the diffusion radius increases over time, and is preset by the simulation environment. This indicates the magnitude of the local wind speed output by the wind disturbance module, i.e. The magnitude of the diffusion value increases with wind speed, and the faster the diffusion occurs. When the environment is windless... The diffusion radius degenerates to its initial value. The above-mentioned coal dust diffusion model is only used to simulate the true values ​​of dust fields with different concentration distributions in the environment; in actual deployment, the measurement values ​​at the current location are directly obtained by airborne dust concentration sensors, without relying on this diffusion model.

[0054] The dynamic obstacle model compares point cloud data between consecutive frames to determine whether the movement speed of a point cloud exceeds a preset threshold. If so, it is marked as a dynamic obstacle.

[0055] In this embodiment, to verify the positioning effect of the method of the present invention in a complex coal mine environment, a simulation environment was set up based on the wind disturbance model and the coal dust diffusion model. The UAV was equipped with a lidar 1, an inertial measurement unit (IMU) 2, and a dust concentration sensor 3, and flew in a simulated roadway 4. Four operating conditions were set: normal environment, high dust environment, strong wind disturbance environment, and comprehensive interference environment. In the normal environment, no significant dust, wind disturbance, or dynamic obstacle interference was introduced; in the high dust environment, coal dust concentration changes were introduced in local roadway areas to simulate the impact of dust obstruction on the observation reliability of lidar 1; in the strong wind disturbance environment, wind disturbance was introduced in local areas to simulate the impact of airflow changes on the inertial prediction process; and in the comprehensive interference environment, dust, wind disturbance, and dynamic obstacle interference were introduced simultaneously to simulate the complex positioning scenario under the combined action of multiple disturbances in a coal mine roadway.

[0056] Using the actual pose of the UAV output from the simulation environment as the reference ground truth, the positioning was compared using the traditional fixed-parameter iEKF method and the adaptive iEKF method described in this invention. The root mean square error (RMSE) of the position was used as the evaluation index for positioning accuracy. The simulation results are as follows: Figure 3 As shown. Under normal conditions, the root mean square errors of the two methods are basically the same. This is because, in the absence of significant interference, the positioning accuracy of the present invention approaches the reference value, and the filter behavior is basically equivalent to that of the fixed parameter method, indicating that the present invention does not introduce significant additional positioning errors in low-interference environments. As the level of interference increases, the advantages of the present invention gradually become apparent: in a high-dust environment, the root mean square error of the position decreases from 0.327m to 0.249m, reflecting the point-level confidence weights constructed in the present invention. The noise level decreases adaptively with increasing dust concentration, and the point-level confidence weight effectively increases the measurement noise covariance of the corresponding observation point and reduces the pollution observation weight. Under strong wind disturbance, the noise level decreases from 0.285m to 0.194m, demonstrating the effectiveness of the process noise covariance adaptively adjusting with disturbance intensity and suppressing IMU prediction drift constructed in this invention. Under comprehensive disturbance, the noise level decreases from 0.521m to 0.368m, indicating that this invention improves overall positioning robustness by defining dynamic obstacle labels to remove point clouds and through the synergistic effect of posterior covariance environmental constraints. These results demonstrate that the method of this invention, by incorporating dust concentration, disturbance intensity, and dynamic obstacle information into the noise adaptive adjustment mechanism of the filtering process, can effectively suppress the influence of abnormal observations and model disturbances on positioning results under multiple disturbance conditions. This achieves a synergistic improvement in the positioning accuracy and stability of UAVs in complex coal mine environments, verifying the effectiveness and robustness of the positioning method of this invention.

[0057] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and not to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some or all of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A tightly coupled adaptive filtering positioning method for unmanned aerial vehicles (UAVs) in a coal mine environment, characterized in that, Includes the following steps: Step 1: Acquire the lidar data and inertial navigation data for the current filtering cycle, and perform coordinate transformation and time synchronization processing to obtain a standardized data packet; at the same time, acquire environmental data, including dust concentration data, disturbance intensity data, and dynamic obstacle tags; Step 2: Obtain the initial state value and its covariance matrix of the current filtering cycle from the inertial navigation data. Perform forward propagation based on the disturbance intensity and dust concentration to obtain the prior state containing attitude, velocity and position. At the same time, construct the process noise covariance matrix based on the disturbance intensity and dust concentration, and perform forward propagation through the process noise covariance matrix to obtain the prior covariance matrix. Step 3: Based on the prior state, perform intra-frame motion compensation on the point cloud set in the lidar data to obtain the coordinate correction value of each point cloud in the reference time coordinate system, and calculate the point-level confidence weight coefficient to obtain the weighted distortion-free point cloud set. Step 4: Remove point clouds labeled as dynamic obstacles, use the remaining point clouds as observation points, and calculate the normal geometric residual, observation Jacobian matrix and single-point measurement noise covariance of each observation point by combining the coordinate correction value and point-level confidence weight coefficient in the weighted distortion-free point cloud set, and construct the observation set. Step 5: Using the observation set and the prior state and prior covariance matrix obtained from forward propagation as input, calculate the Kalman gain matrix and correct the state and covariance. After correction, apply environmental constraints to the posterior covariance matrix to finally obtain the posterior state vector and posterior covariance matrix of the fused observations. Step 6: Repeat steps 1-5 in the next filtering cycle to achieve continuous positioning of the UAV.

2. The tightly coupled adaptive filtering positioning method for UAVs in a coal mine environment according to claim 1, characterized in that, In step 1, the specific method for coordinate transformation and time synchronization of lidar data and inertial navigation data is as follows: When timing is assigned to LiDAR data and inertial navigation data under a unified clock, if the time difference between a single frame of LiDAR data and adjacent inertial navigation data is greater than a threshold, linear interpolation resampling is performed on the inertial navigation data so that each frame of LiDAR data obtains the attitude and velocity at the corresponding moment, and the scanning span and point-level timestamp of each frame of data are recorded. Then, the point cloud of the LiDAR data is converted to the inertial navigation coordinate system using the extrinsic parameter matrix obtained through offline calibration.

3. The tightly coupled adaptive filtering positioning method for UAVs in a coal mine environment according to claim 1, characterized in that, In step 1, the specific method for obtaining dust concentration data, disturbance intensity data, and dynamic obstacle tags is as follows: Obtain dust concentration data output by the dust sensor, and perform nearest neighbor or linear interpolation based on the timestamp to obtain the dust concentration data corresponding to the point-level timestamp of each frame of data; The disturbance intensity is calculated based on the residual statistics of the previous filtering cycle; Determine if there are two frames of point cloud in the LiDAR data whose velocity exceeds the threshold. If so, mark them as dynamic obstacles.

4. The tightly coupled adaptive filtering positioning method for UAVs in a coal mine environment according to claim 1, characterized in that, In step 2, the formula for calculating the prior state is: ； in, This represents the prior state of the current filtering cycle. This represents the posterior state of the previous filtering cycle. This represents the nonlinear function corresponding to the state equation, where This represents the state corresponding to time t; This represents the inertial navigation data corresponding to time t; The process noise representing the external disturbance is combined with the disturbance intensity during the integration process. Adaptive adjustment of the integral step size; The motion state equation of the UAV is expressed as: ； The derivative of the position p of the UAV in the global coordinate system is given by: Represents the velocity vector The derivative of Indicates acceleration. The attitude rotation matrix represents the rotation from the machine coordinate system to the global coordinate system. express The derivative, and These represent the bias terms for the accelerometer and gyroscope, respectively. This indicates the noise measured by the accelerometer. Represents the gravitational acceleration vector; Indicates angular velocity. Indicates gyroscope noise. Represents the mapping operation for antisymmetric matrices; The formulas for calculating the process noise covariance matrix and the prior covariance matrix are as follows: ； in, This represents the process noise covariance matrix corresponding to the previous filtering cycle. Indicates the baseline covariance. , This represents the adjustment coefficient. This represents the average value of the dust sensor readings over the previous filtering cycle. This represents the trace of the disturbance intensity during the previous filtering cycle. This represents the prior covariance of the current filtering cycle. and These represent the state transition matrix and noise transfer matrix corresponding to the previous filtering cycle, respectively. This represents the posterior covariance matrix corresponding to the previous filtering cycle. This represents the transpose of a matrix.

5. The tightly coupled adaptive filtering positioning method for UAVs in a coal mine environment according to claim 1, characterized in that, In step 3, the weighted distortion-free point cloud set is represented as: ； Among them are: ； ； This represents a weighted set of distortion-free point clouds. Indicates the first The coordinate correction values ​​of a point cloud in the reference time coordinate system after motion compensation. Represents the confidence weight coefficient of the i-th point cloud; and Representing reference time and sampling time The attitude rotation matrix corresponding to the body coordinate system to the world coordinate system. Indicates the first The original coordinate measurements of the point cloud in the lidar coordinate system and They represent the sampling times respectively. and reference time The position vector of the origin of the machine system in the world coordinate system; This represents the preset dust concentration attenuation coefficient. Indicates the sampling time Dust concentration.

6. The tightly coupled adaptive filtering positioning method for UAVs in a coal mine environment according to claim 1, characterized in that, In step 4, the formulas for calculating the normal geometric residual, the observation Jacobian matrix, and the single-point measurement noise covariance are as follows: ； = ； ； in, , , Let represent the normal geometric residual, observation Jacobian matrix, and single-point measurement noise covariance corresponding to the i-th observation point, respectively; This represents the local normal vector corresponding to the i-th observation point. transpose, This represents the coordinate correction value of the i-th point cloud in the reference time coordinate system after motion compensation. express Orthogonal projection points in a local map This represents the observation derivative information corresponding to the i-th observation point. and Let represent the attitude rotation matrix of the body coordinate system relative to the world coordinate system at the reference time, and the position vector of the origin of the body coordinate system in the world coordinate system, respectively. This represents the reference noise under sensor calibration. This represents the confidence weight coefficient of the i-th point cloud. This is a preset positive small constant used to avoid the denominator being zero.

7. The tightly coupled adaptive filtering positioning method for UAVs in a coal mine environment according to claim 1, characterized in that, In step 5, the environmental constraints applied to the posterior covariance matrix are as follows: ； in, and These represent the posterior covariance matrices before and after applying environmental constraints in the current filtering cycle, respectively. , This represents the adjustment coefficient. This indicates the disturbance intensity in the current filtering period. This represents the average value of the dust sensor readings within the current filtering cycle. It represents the identity matrix of the same order as the posterior covariance matrix.

8. The tightly coupled adaptive filtering positioning method for UAVs in a coal mine environment according to claim 1, characterized in that, In step 5, the formula for calculating the Kalman gain matrix and performing state and covariance correction is as follows: ； in, , Let these represent the prior state vector and prior covariance matrix before observation fusion in the current filtering cycle, respectively. , This represents the posterior state vector and posterior covariance matrix after fusion of observations. This represents the observation residual vector constructed from all observations in the current filtering period. To obtain the observed Jacobian matrix linearized at the prior state, This is the corresponding measurement noise covariance matrix. Here is the Kalman gain matrix. It is the identity matrix. Indicates matrix transpose operation, subscript Used to indicate the current filtering period.

9. A tightly coupled adaptive filtering positioning method for UAVs in a coal mine environment according to claim 1, characterized in that, It also includes the following steps: Step 7: Optimize decision-making based on posterior state variables and posterior covariance matrix to achieve UAV control.

10. A tightly coupled adaptive filtering positioning method for UAVs in a coal mine environment according to claim 9, characterized in that, In step 7, decision optimization is performed based on near-end strategy optimization.

Images