Positioning and compensation method and system of unmanned aerial vehicle in underground space environment
By constructing a Hessian matrix and eigenvalue decomposition, and combining visual observation constraints and dynamic weight adjustment, the positioning drift problem of UAVs in underground space environments was solved, achieving high-precision and stable UAV flight.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- CHINA CONSTR EIGHTH ENG DIV CORP LTD ZHEJIANG CONSTR CO LTD
- Filing Date
- 2026-03-12
- Publication Date
- 2026-06-12
AI Technical Summary
Drones in underground environments suffer from positioning drift and flight instability due to the lack of axial constraints. This is especially true in environments with simple geometric features, such as tunnels, where laser inertial odometry cannot effectively detect axial displacement, making it difficult to hover in one place and posing a risk of collision.
Multi-source sensor data is collected using lidar, inertial measurement unit, and depth camera. Eigenvalue decomposition is performed by constructing a Hessian matrix, the weights of lidar and visual observations are dynamically adjusted, axial drift correction is performed by combining visual observation constraints, and a two-way verification mechanism is established to remove abnormal data, thereby achieving visual compensation and fusion positioning.
It improves positioning accuracy and flight stability in underground environments, avoids positioning drift and environmental interference, and ensures safe flight of UAVs in complex industrial environments.
Smart Images
Figure CN122192307A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of unmanned aerial vehicle (UAV) positioning technology, and specifically to a method and system for positioning and compensating UAVs in underground space environments. Background Technology
[0002] With the increasing demand for industrial inspection, autonomous flight and positioning technology of drones in complex environments such as underground spaces and tunnels has become a research hotspot. In underground environments, drones cannot rely on GPS signals for positioning and usually use laser inertial odometry (LIO) to achieve autonomous positioning.
[0003] However, underground spaces such as tunnels are characterized by highly homogeneous geometric features. For example, long straight tunnels only have sidewall constraints and lack front and rear plane constraints along the direction of movement. When a UAV moves along the tunnel axis, the residual of the lidar scan matching is extremely small, which makes it impossible for the laser inertial odometry to effectively sense the axial displacement, resulting in severe axial positioning drift. This makes it difficult for the UAV to hover in place and makes it very easy to have a collision, which seriously affects the safety and reliability of inspection operations. Summary of the Invention
[0004] The purpose of this invention is to overcome the shortcomings of the prior art and provide a method and system for positioning and compensation of unmanned aerial vehicles (UAVs) in underground space environments, thereby solving the problems of poor positioning accuracy and flight stability of UAVs in underground environments in the prior art.
[0005] To achieve the above objectives, this invention provides a positioning and compensation method for a drone in an underground space environment, based on the drone being equipped with a lidar, an inertial measurement unit, and a camera; the positioning and compensation method includes the following steps: Acquire multi-source sensor data collected by the lidar, inertial measurement unit, and depth camera; Based on the sensing data of the lidar and the inertial measurement unit, a scanning match is performed to construct a Hessian matrix and perform eigenvalue decomposition, with the smallest eigenvalue obtained as the degradation factor. When the degradation factor is lower than a preset robustness threshold, geometric feature degradation is determined to have occurred, and the degradation axis is locked. Visual feature points are extracted from the images acquired by the depth camera to obtain visual observation constraints; The weights of laser observation and visual observation are dynamically adjusted based on the degradation factor, and the drift of laser positioning on the degradation axis is corrected by the visual observation constraint, and the fused pose information is output.
[0006] By adopting this technical solution, a degradation factor is obtained by constructing a Hessian matrix and performing eigenvalue decomposition. When the degradation factor is lower than a preset robustness threshold, visual compensation is actively triggered. This enables the active locking of the degradation axis and the triggering of pose compensation before positioning drift occurs. This solves the positioning drift problem caused by the lack of axial constraints in laser inertial odometry in environments with simple geometric features such as tunnels, and improves the positioning accuracy and flight stability of UAVs in underground space environments.
[0007] Furthermore, the locking of the degradation axis specifically means: locking the direction of the eigenvector corresponding to the minimum eigenvalue as the axis on which geometric feature degradation occurs.
[0008] By adopting this technical solution, specific degradation dimensions can be identified through feature vector direction, enabling visual compensation to be precisely applied to the axis where drift occurs, avoiding redundant compensation and improving the accuracy of pose correction.
[0009] Furthermore, the dynamic adjustment of the weights of laser observation and visual observation specifically involves: dynamically reconstructing the observation noise covariance matrix in the extended Kalman filter based on the degradation factor, increasing the covariance value of laser observation and decreasing the covariance value of visual observation along the degradation axis.
[0010] By adopting this technical solution, the dynamic and smooth adjustment of laser and visual weights is achieved, avoiding the problems of sluggish perception of environmental changes and abrupt switching in traditional solutions, thus ensuring the stability of fusion positioning.
[0011] Furthermore, the visual feature points are specifically ORB feature points; The obtained visual observation constraints include combining the depth information from the depth camera to restore the visual feature points to three-dimensional points in the camera coordinate system.
[0012] By adopting this technical solution, ORB feature points have good rotation invariance and computational efficiency. Combining depth information to recover 3D points can provide accurate geometric information for visual observation constraints, laying the foundation for subsequent reprojection residual calculation and pose correction.
[0013] Furthermore, it also includes a two-way verification step: real-time comparison of visual motion vectors and laser displacement increments; when the laser pose changes abruptly while the visual motion vectors remain smooth and consistent, abnormal laser data is discarded.
[0014] By adopting this technical solution, laser flying points caused by water surface reflection, dust or strong light interference can be effectively eliminated, abnormal data can be avoided from interfering with the positioning system, and the anti-interference ability and robustness in complex industrial environments can be significantly improved.
[0015] This invention also provides a positioning and compensation system for unmanned aerial vehicles (UAVs) in underground space environments, comprising: LiDAR, inertial measurement unit and depth camera are mounted on the drone to collect multi-source sensor data; The degradation detection module is used to construct a Hessian matrix and perform eigenvalue decomposition based on the scanning matching results of the sensing data of the lidar and the inertial measurement unit. The minimum eigenvalue obtained is used as the degradation factor. When the degradation factor is lower than a preset robustness threshold, it is determined that geometric feature degradation has occurred and the degradation axis is locked. The visual compensation module is used to extract visual feature points from the images acquired by the depth camera when geometric feature degradation occurs, so as to obtain visual observation constraints. The fusion positioning module is used to dynamically adjust the weights of laser observation and visual observation based on the degradation factor, use the visual observation constraints to correct the drift of laser positioning on the degradation axis, and output the fused pose information.
[0016] By adopting this technical solution, the degradation detection module, visual compensation module, and fusion positioning module work together to achieve real-time detection of laser feature degradation and dynamic compensation of visual pose, providing complete hardware and software support for high-precision positioning of UAVs in underground space environments.
[0017] Furthermore, the fusion positioning module employs extended Kalman filtering to dynamically reconstruct the observation noise covariance matrix based on the degradation factor, increasing the covariance value of laser observations and decreasing the covariance value of visual observations along the degradation axis.
[0018] By adopting this technical solution, extended Kalman filtering can effectively fuse multi-source heterogeneous sensor data and dynamically reconstruct the covariance matrix to achieve adaptive weight adjustment based on environmental perception, ensuring that visual observation can timely and effectively constrain the axial drift of laser positioning in degraded scenarios.
[0019] Furthermore, it also includes an anomaly removal module, which compares the visual motion vector with the laser displacement increment. When the laser pose changes abruptly while the visual motion vector remains smooth and consistent, the abnormal laser data is removed.
[0020] By adopting this technical solution, the module serves as a safety redundancy for the system, effectively preventing positioning jumps caused by environmental interference and ensuring the flight safety of UAVs during underground space inspection operations.
[0021] Compared with the prior art, the present invention has the following advantages: 1. By constructing a Hessian matrix and performing eigenvalue decomposition to obtain a degradation factor, visual compensation is actively triggered when the degradation factor is lower than a preset robustness threshold. This enables the active locking of the degradation axis and triggering pose compensation before positioning drift occurs, thereby solving the positioning drift problem caused by the lack of axial constraints in laser inertial odometry in environments with simple geometric features such as tunnels, and improving the positioning accuracy and flight stability of UAVs in underground space environments.
[0022] 2. Based on the degradation factor, the weights of laser observation and visual observation are dynamically adjusted. On the degradation axis, the covariance of laser observation is increased to reduce its trust weight, and the covariance of visual observation is decreased to increase its constraint gain. This achieves a smooth transition and fine-grained adjustment of the positioning weight, avoiding the problems of sluggish perception of environmental changes and abrupt switching in traditional schemes.
[0023] 3. A two-way verification mechanism for multi-source data has been established. By comparing the consistency between visual motion vectors and laser displacement increments in real time, abnormal laser flying point data caused by water surface reflection, dust or strong light interference can be effectively identified and eliminated, which significantly improves the anti-interference capability and robustness of the positioning system in complex industrial environments. Attached Figure Description
[0024] Figure 1 This is a flowchart illustrating the method for locating and compensating unmanned aerial vehicles (UAVs) in an underground space environment, as described in this embodiment of the invention. Detailed Implementation
[0025] The present invention will be further described below with reference to the accompanying drawings and specific embodiments.
[0026] Please see the appendix Figure 1 This invention provides a method for positioning and compensation of unmanned aerial vehicles (UAVs) in underground space environments. This method is applied to UAVs equipped with three-dimensional solid-state lidar, inertial measurement unit (IMU) and D455 depth camera. The UAV establishes a global inertial coordinate system with the centroid at the moment of self-test as the origin. Through a timestamp hard triggering mechanism, the lidar point cloud (30Hz), visual image (30Hz) and IMU data (above 200Hz) are uniformly mapped to the global coordinate system to achieve spatiotemporal synchronization of multi-source data.
[0027] The positioning and compensation method in this embodiment includes the following steps: Step 1: Acquire multi-source sensor data collected by the lidar, inertial measurement unit, and depth camera.
[0028] Step 2: Based on the sensing data of the lidar and the inertial measurement unit, perform scanning matching, construct a Hessian matrix and perform eigenvalue decomposition, and use the obtained minimum eigenvalue as the degradation factor.
[0029] Specifically, when performing scan matching, the laser inertial odometry unit constructs the Hessian matrix H for the current registration problem, performs eigenvalue decomposition on matrix H, and extracts its minimum eigenvalue. s min Define the degradation factor D = s min In long, straight tunnels, due to the lack of planar constraints along the axis of motion, s min It will decrease significantly, leading to a decrease in the degradation factor D.
[0030] Step 3: When the degradation factor is lower than the preset robustness threshold, it is determined that geometric feature degradation has occurred, and the degradation axis is locked.
[0031] Specifically, degradation is determined by comparing the quantification relationship between the degradation factor D and the preset robustness threshold T; when D < T, the system determines that geometric feature degradation has occurred and immediately locks the minimum eigenvalue. s min The corresponding feature vector direction is used as the degradation axis, and the visual compensation mechanism is activated.
[0032] Step four: Extract visual feature points from the images acquired by the depth camera to obtain visual observation constraints.
[0033] Specifically, ORB feature points are extracted from the image stream of the D455 depth camera, and combined with the depth information of the depth camera, the visual feature points are restored to three-dimensional points in the camera coordinate system. P cam , as a constraint for visual observation.
[0034] Step 5: Based on the degradation factor, dynamically adjust the weights of laser observation and visual observation, use the visual observation constraint to correct the drift of laser positioning on the degradation axis, and output the fused pose information.
[0035] Specifically, within the Extended Kalman Filter (EKF) framework, the observation noise covariance matrix R is reconstructed in real time based on the dynamic changes of the degradation factor D. Along the detected degradation axis, the covariance value corresponding to the laser observation is significantly increased to reduce its trust weight, while the covariance value corresponding to the visual observation is decreased to improve its constraint gain. The reprojection residual of the visual feature points is calculated as an innovative term of the filter and injected into the system. Through the texture constraints of vision, the geometric deficiencies of the laser are compensated, and the cumulative drift generated by the laser odometry along the degradation axis is forcibly corrected.
[0036] Step 6: Compare the visual motion vector with the laser displacement increment in real time. When the laser pose changes abruptly while the visual motion vector remains smooth and consistent, the abnormal laser data is discarded.
[0037] Specifically, to prevent interference in the industrial environment (such as water surface reflection, dust, and strong light), a two-way verification mechanism is established to compare the displacement increment of the visual motion vector with that of the laser output in real time. If the lidar generates flying points that cause a sudden change in pose, while the motion trajectory of the visual feature points remains smooth and consistent, the system automatically masks and removes the abnormal laser data of that frame to maintain the continuity of the positioning trajectory.
[0038] Step 7: Output the corrected six-DOF pose, including position information X, Y, Z and attitude information Roll, Pitch, Yaw, and feed it back to the flight controller in real time. By comparing the deviation between the current pose and the target trajectory, the motor PWM signal is adjusted in real time to achieve closed-loop fixed-point hovering and stable flight of the UAV in the underground space environment. This invention also provides a positioning and compensation system for unmanned aerial vehicles (UAVs) in underground space environments, comprising: The lidar, inertial measurement unit, and depth camera are mounted on the drone to collect multi-source sensor data; the lidar is a 3D solid-state lidar, and the depth camera is a D455 depth camera.
[0039] The degradation detection module is used to construct a Hessian matrix and perform eigenvalue decomposition based on the scanning matching results of the sensing data of the lidar and the inertial measurement unit. The minimum eigenvalue obtained is used as the degradation factor. When the degradation factor is lower than the preset robustness threshold, it is determined that geometric feature degradation has occurred and the direction of the eigenvector corresponding to the minimum eigenvalue is locked as the degradation axis.
[0040] The visual compensation module is used to extract ORB feature points from the images acquired by the depth camera as visual feature points when geometric feature degradation occurs. It combines the depth information to restore the visual feature points as three-dimensional points in the camera coordinate system, thus obtaining visual observation constraints.
[0041] The fusion positioning module employs an extended Kalman filter to dynamically reconstruct the observation noise covariance matrix based on the degradation factor. It increases the covariance value of laser observations and decreases the covariance value of visual observations along the degradation axis. It uses visual observation constraints to correct the drift of laser positioning along the degradation axis and outputs the fused six-degree-of-freedom pose information.
[0042] The anomaly removal module compares the visual motion vector with the laser displacement increment in real time. When the laser pose changes abruptly while the visual motion vector remains smooth and consistent, the current laser data is determined to be a flying point caused by environmental interference, and masking is performed to remove it. The environmental interference includes water surface reflection, dust, or strong light.
[0043] The flight control module receives six-degree-of-freedom pose information output by the fusion positioning module. By comparing the deviation between the current pose and the target trajectory, it adjusts the PWM signals of the four motors in real time to achieve closed-loop control of the UAV. The present invention has been described in detail above with reference to the accompanying drawings and embodiments. Those skilled in the art can make various modifications to the present invention based on the above description. Therefore, certain details in the embodiments should not be construed as limiting the present invention, and the scope of protection of the present invention shall be defined by the appended claims.
Claims
1. A method for positioning and compensation of a drone in an underground space environment, based on the drone being equipped with a lidar, an inertial measurement unit, and a camera; characterized in that, The positioning and compensation method includes the following steps: Acquire multi-source sensor data collected by the lidar, inertial measurement unit, and depth camera; Based on the sensing data of the lidar and the inertial measurement unit, a scanning match is performed to construct a Hessian matrix and perform eigenvalue decomposition, with the smallest eigenvalue obtained as the degradation factor. When the degradation factor is lower than a preset robustness threshold, geometric feature degradation is determined to have occurred, and the degradation axis is locked. Visual feature points are extracted from the images acquired by the depth camera to obtain visual observation constraints; The weights of laser observation and visual observation are dynamically adjusted based on the degradation factor, and the drift of laser positioning on the degradation axis is corrected by the visual observation constraint, and the fused pose information is output.
2. The method for positioning and compensating unmanned aerial vehicles (UAVs) in an underground space environment according to claim 1, characterized in that, The locking degradation axis specifically means: locking the direction of the eigenvector corresponding to the minimum eigenvalue as the axis on which geometric feature degradation occurs.
3. The method for positioning and compensating unmanned aerial vehicles in an underground space environment according to claim 1, characterized in that, The specific method for dynamically adjusting the weights of laser observation and visual observation is as follows: in the extended Kalman filter, the observation noise covariance matrix is dynamically reconstructed according to the degradation factor, and the covariance value of laser observation is increased and the covariance value of visual observation is decreased along the degradation axis.
4. The method for positioning and compensating unmanned aerial vehicles (UAVs) in an underground space environment according to claim 1, characterized in that, The visual feature points are specifically ORB feature points; The obtained visual observation constraints include combining the depth information from the depth camera to restore the visual feature points to three-dimensional points in the camera coordinate system.
5. The method for positioning and compensating unmanned aerial vehicles (UAVs) in an underground space environment according to claim 1, characterized in that, It also includes a two-way verification step: real-time comparison of visual motion vectors and laser displacement increments. When the laser pose changes abruptly while the visual motion vectors remain smooth and consistent, abnormal laser data is discarded.
6. A positioning and compensation system for unmanned aerial vehicles (UAVs) in an underground space environment, used to implement the positioning and compensation method for UAVs in an underground space environment as described in any one of claims 1-5, characterized in that, The positioning and compensation system includes: LiDAR, inertial measurement unit and depth camera are mounted on the drone to collect multi-source sensor data; The degradation detection module is used to construct a Hessian matrix and perform eigenvalue decomposition based on the scanning matching results of the sensing data of the lidar and the inertial measurement unit. The minimum eigenvalue obtained is used as the degradation factor. When the degradation factor is lower than a preset robustness threshold, it is determined that geometric feature degradation has occurred and the degradation axis is locked. The visual compensation module is used to extract visual feature points from the image acquired by the depth camera when geometric feature degradation occurs, so as to obtain visual observation constraints. The fusion positioning module is used to dynamically adjust the weights of laser observation and visual observation based on the degradation factor, use the visual observation constraints to correct the drift of laser positioning on the degradation axis, and output the fused pose information.
7. The positioning and compensation system for unmanned aerial vehicles in an underground space environment according to claim 6, characterized in that, The fusion positioning module employs extended Kalman filtering to dynamically reconstruct the observation noise covariance matrix based on the degradation factor, increasing the covariance value of laser observations and decreasing the covariance value of visual observations along the degradation axis.
8. The positioning and compensation system for unmanned aerial vehicles in an underground space environment according to claim 6, characterized in that, It also includes an anomaly removal module, which compares the visual motion vector with the laser displacement increment. When the laser pose changes abruptly while the visual motion vector remains smooth and consistent, the abnormal laser data is removed.