Optimization methods, optimization systems, and computer-readable storage media for underground power plants based on UAV Fast LIO technology

Abstract

The application provides an optimization method and system based on a UAV Fast LIO technology in an underground plant and a storage medium, and belongs to the technical field of three-dimensional environment perception. The method aims to solve the problems of low positioning accuracy and existence of cumulative drift of the UAV in the environment of the underground plant and the like where GPS fails and metal interference is strong. The method comprises: pre-processing point cloud to filter interference; constructing a prior map containing a stable structure; in the Fast LIO framework, matching the current frame features with the prior map, generating a high-weight constraint and adding a Kalman filter for optimization. The application fuses global prior information, effectively suppresses positioning errors, significantly improves positioning accuracy and robustness, eliminates cumulative drift, and ensures autonomous inspection of the UAV.

Description

Technical Field

[0001] This invention relates to the field of three-dimensional environmental perception technology, specifically to an optimization method, optimization system, and computer-readable storage medium for underground power plants based on UAV Fast LIO technology. Background Technology

[0002] The underground powerhouse of a pumped-storage power station is a crucial location housing core power generation equipment such as pumps, turbines, and generators. Its internal structure is complex and vast, filled with numerous metal devices, pipes, and steel structures. Regular inspections of the underground powerhouse to monitor equipment status and structural safety are essential for ensuring the stable operation of the power station.

[0003] Traditional manual inspection methods have several shortcomings. First, manual inspections are inefficient and difficult to reach tall and complex equipment areas, easily creating blind spots. Second, workers must work near large operating equipment, posing significant safety hazards. Furthermore, inspection results rely on manual recording, which is prone to errors and makes systematic data management and long-term comparative analysis difficult, failing to meet the needs of modern power plant operation and maintenance management.

[0004] In recent years, the development of drone inspection technology has provided new solutions to the aforementioned problems. However, applying drone technology in the special environment of underground factories faces severe technical challenges. The underground environment renders GPS signals completely ineffective, forcing drones to rely entirely on onboard sensors for autonomous positioning and navigation. Simultaneously, the numerous metal surfaces within the factory, including equipment casings, steel structures, and metal pipes, generate strong specular reflections or multipath effects on lidar signals, producing a large number of noise points and false data, severely interfering with lidar SLAM-based positioning algorithms.

[0005] The repetitive nature of environmental structures also presents additional challenges for UAV localization. Factories often contain numerous similar structural units, such as regularly arranged beams and columns. This high degree of structural similarity can easily lead to confusion in feature matching by localization algorithms, causing localization drift or failure. Furthermore, the presence of moving personnel or equipment within the factory can negatively impact the map building and localization stability of the SLAM system, further increasing the complexity of its implementation.

[0006] Fast LIO (Fast LiDAR-Inertial Odometry) technology, as an advanced laser inertial odometry technique, exhibits good robustness in highly dynamic and sparse environments by tightly coupling and fusing data from lidar and inertial measurement units. This technology employs an iterative error state Kalman filter to achieve deep fusion of sensor data and utilizes efficient data structures for feature extraction and matching. However, the general-purpose Fast LIO algorithm is primarily designed for conventional environments and does not consider the strong metal reflection interference commonly found in industrial environments, nor is it specifically optimized for the structured characteristics of underground facilities.

[0007] When the existing Fast LIO algorithm is directly applied in underground factory environments, it generates a large number of erroneous feature points due to signal interference from metal surfaces, leading to a significant decrease in positioning accuracy. Furthermore, due to the lack of effective utilization of environmental structural features, the algorithm is prone to accumulating errors in repetitive structural areas, making it difficult to guarantee positioning stability for long-term inspection tasks. These problems severely restrict the practical application of UAV technology in complex industrial environments such as underground factories. Summary of the Invention

[0008] The purpose of this invention is to provide an optimization method, optimization system, and computer-readable storage medium for underground powerhouses based on Fast LIO technology for unmanned aerial vehicles (UAVs). This method can effectively suppress metal signal interference and fully utilize environmental structural features to enhance positioning robustness, thereby achieving safe, efficient, and intelligent operation and maintenance management of underground powerhouses and promoting the digital transformation of pumped storage power stations.

[0009] To achieve the above objectives, the present invention provides the following technical solution:

[0010] An optimization method for underground power plants based on Fast LIO technology for unmanned aerial vehicles includes the following steps:

[0011] S1: Acquire raw point cloud data and inertial data collected by a drone equipped with a lidar and inertial measurement unit in an underground factory environment;

[0012] S2: Preprocess the original point cloud data to filter out interference points generated by reflection from the metal surface;

[0013] S3: Based on the Fast LIO framework, feature extraction and matching are performed by fusing preprocessed point cloud data and inertial data through an iterative error state Kalman filter.

[0014] S4: Construct a priori structural map of the underground plant, wherein the priori structural map contains geometric primitive information of the regular structure within the plant;

[0015] S5: Match the features extracted from the current frame with the prior structure map to generate matching constraints;

[0016] S6: Add the matching constraints to the state optimization of the iterative error state Kalman filter to obtain the optimized UAV pose estimation and environment map.

[0017] Further: the preprocessing of the raw point cloud data in step S2 includes:

[0018] Based on the echo intensity information of lidar, points with abnormally high intensity values ​​that do not conform to the surrounding geometry are identified and filtered out.

[0019] An adaptive radius filtering algorithm is used to remove spatially sparse outliers;

[0020] A statistical outlier removal algorithm is applied to remove isolated noise points caused by specular reflection or multipath effects on metal surfaces.

[0021] Further: The method for constructing the prior structure map in step S4 includes:

[0022] An initial scan of the underground factory building was performed to obtain initial point cloud data;

[0023] The RANSAC algorithm is used to extract planar and linear geometric primitives from the initial point cloud data;

[0024] The extracted geometric primitives are stored in a parametric form, where planes are stored in the form of plane equations and lines are stored in the form of line equations.

[0025] Furthermore: the planar geometric primitives include the walls, floor, and ceiling of the factory building; the linear geometric primitives include the edges of beams and columns and the corner lines.

[0026] Further: the matching process in step S5 includes:

[0027] Extract planar features and edge features from the current frame point cloud;

[0028] Search the prior structure map for geometric primitives corresponding to the features of the current frame;

[0029] Calculate the residual between the current frame features and the corresponding geometric primitives in the prior map.

[0030] Further: In step S6, the residual is added as a high-weight constraint term to the objective function of the Levenberg-Marquardt optimization algorithm.

[0031] Furthermore, it also includes:

[0032] S7: Based on the optimized environment map, segment point cloud clusters representing large equipment in real time;

[0033] S8: Compare the current position of the device point cloud cluster with the baseline position to detect whether the device has shifted or deformed.

[0034] Furthermore, in step S3, the Fast LIO framework uses an iKD-Tree data structure for feature storage and nearest neighbor search, and utilizes IMU pre-integration to perform motion compensation and distortion correction on the lidar point cloud.

[0035] This invention also provides an optimization system for underground power plants based on Unmanned Aerial Vehicle (UAV) Fast LIO technology, comprising:

[0036] The data acquisition module is used to acquire raw point cloud data and inertial data collected by a drone equipped with a lidar and an inertial measurement unit in an underground factory environment.

[0037] The point cloud preprocessing module is used to filter the original point cloud data to remove interference points caused by reflection from the metal surface.

[0038] The data fusion module is used to fuse preprocessed point cloud data and inertial data using an iterative error state Kalman filter based on the Fast LIO framework.

[0039] The prior map construction module is used to construct a prior structure map containing information on the geometric primitives of the regular structure within the factory building;

[0040] The feature matching module is used to match the features extracted from the current frame with the prior structure map to generate matching constraints;

[0041] The state optimization module is used to add the matching constraints to the state optimization of the iterative error state Kalman filter, and output the optimized UAV pose estimate and environment map.

[0042] The present invention also provides a computer-readable storage medium storing a computer program that, when executed by a processor, implements the above-described method.

[0043] Compared with the prior art, the present invention has the following advantages:

[0044] I. Significantly Improved Positioning Accuracy in Complex Metal Environments. By introducing a multi-layered point cloud preprocessing mechanism, including echo intensity-based filtering, adaptive radius filtering, and statistical outlier removal, interference points caused by specular reflection from metal surfaces and multipath effects are effectively eliminated, ensuring the quality of point cloud data fed into the Fast LIO system from the source. Experiments show that compared to directly applying the original Fast LIO algorithm, the method of this invention effectively reduces positioning errors in underground factory environments.

[0045] Second, it solves the cumulative drift problem in structured environments. By constructing and utilizing a prior structure map, stable regular structures such as walls, floors, beams, and columns within the factory building are used as global positioning benchmarks and integrated into the state optimization process as high-weight constraints. This method effectively overcomes positioning confusion caused by similar or repetitive local features, ensuring global consistency of positioning during long-term inspection tasks.

[0046] Third, stable autonomous navigation in GPS-free environments is achieved. The method of this invention does not rely on GPS signals at all. It achieves centimeter-level positioning accuracy in environments such as underground factories where GPS is completely ineffective, by tightly coupling and fusing lidar and IMU, combined with environmental structural constraints. This provides a reliable technical guarantee for the autonomous operation of UAVs in complex indoor industrial environments.

[0047] Fourth, stable autonomous navigation in GPS-free environments is achieved. The method of this invention does not rely on GPS signals at all. It achieves centimeter-level positioning accuracy in environments such as underground factories where GPS is completely ineffective, by tightly coupling and fusing lidar and IMU, combined with environmental structural constraints. This provides a reliable technical guarantee for the autonomous operation of UAVs in complex indoor industrial environments.

[0048] V. Significantly improves inspection efficiency and safety. Compared with traditional manual inspection, the method of this invention increases inspection efficiency by more than 5 times, while completely avoiding the safety risks of operators in high-risk environments, and realizing automated inspection with full coverage and no blind spots in underground plants. Attached Figure Description

[0049] Figure 1 This is a flowchart illustrating one embodiment of the optimization method of the present invention based on the Fast LIO technology of unmanned aerial vehicles in an underground power plant.

[0050] Figure 2 This is a flowchart illustrating another embodiment of the optimization method of the present invention based on UAV Fast LIO technology in an underground factory.

[0051] Figure 3 This is a schematic diagram of the optimized system framework of the present invention based on the Fast LIO technology of unmanned aerial vehicles in an underground factory, as shown in one embodiment. Detailed Implementation

[0052] The technical solution of the present invention will now be clearly and completely described with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. 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.

[0053] In the description of this invention, it should be noted that the terms "center," "upper," "lower," "left," "right," "vertical," "horizontal," "inner," and "outer," etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. They are used only for the convenience of describing the invention and for simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limitations on the invention. Furthermore, the terms "first," "second," and "third" are used for descriptive purposes only and should not be construed as indicating or implying relative importance.

[0054] This invention provides an optimization method for underground powerhouses based on UAV Fast LIO technology. This method is particularly suitable for the inspection and modeling of large and complex industrial facilities such as underground powerhouses of pumped storage power stations. In practical implementation, a UAV platform integrating a lidar and an inertial measurement unit (IMU) is first required. The lidar is preferably a 16-line or 32-line multi-line lidar with a scanning frequency of no less than 10Hz to ensure the density and real-time performance of the point cloud data. The IMU should be an industrial-grade high-precision IMU with a gyroscope zero-bias stability better than 0.1° / h and an accelerometer zero-bias stability better than 50μg, ensuring accurate attitude and acceleration information. The method includes the following steps:

[0055] S1: Acquire raw point cloud data and inertial data collected by a drone equipped with a lidar and inertial measurement unit in an underground factory environment;

[0056] Specifically, during the system initialization phase, a priori structural map of the underground plant needs to be constructed. This process can be achieved in two ways. The first method is to utilize the plant's CAD drawings or architectural blueprints to extract key structural information, including wall locations, beam and column distribution, and floor and ceiling heights, converting this information into a parametric representation of geometric primitives. The second method is to conduct a complete initial scan flight using a drone to collect point cloud data covering the entire plant. When using the initial scan method to construct the priori map, the drone flies within the plant at a relatively slow speed to ensure that the LiDAR can fully scan all areas. During the actual inspection mission, the LiDAR on the drone continuously collects environmental point cloud data, while the IMU outputs attitude and acceleration information at a high frequency.

[0057] S2: Preprocess the original point cloud data to filter out interference points generated by reflection from the metal surface;

[0058] The raw point cloud data first enters the preprocessing module, which is one of the key innovations of this invention. The preprocessing process includes three main steps. In some embodiments, these three steps include:

[0059] S201: Based on the echo intensity information of the lidar, identify and filter out points with abnormally high intensity values ​​that do not conform to the surrounding geometry.

[0060] Specifically, a pass-through filter is applied, with a reasonable distance threshold set, such as 0.5 meters to 50 meters. Points outside this range are considered invalid measurements and are discarded. Intensity-based filtering is then performed, as each point returned by the lidar contains reflection intensity information; specular reflections from metal surfaces often produce abnormally high intensity values.

[0061] S202: Adaptive radius filtering algorithm is used to remove spatially sparse outliers.

[0062] Specifically, an adaptive radius filter is applied. For each point, the number of its neighboring points within the adaptive radius is counted. The radius is dynamically adjusted based on the distance from the point to the sensor, with a larger radius for farther distances. If the number of neighboring points is less than a set threshold, the point is considered an outlier and is removed.

[0063] S203: Apply a statistical outlier removal algorithm to remove isolated noise points caused by specular reflection or multipath effects on metal surfaces. Specifically, by calculating the average intensity of points within a local area, isolated points with intensity values ​​exceeding three standard deviations of the average are marked as potential interference points. If these high-intensity points are spatially discrete rather than on a continuous surface, they are filtered out.

[0064] S3: Based on the Fast LIO framework, feature extraction and matching are performed by fusing preprocessed point cloud data and inertial data through an iterative error state Kalman filter.

[0065] The preprocessed clean point cloud data enters the Fast LIO core algorithm module. Fast LIO fuses LiDAR and IMU data in a tightly coupled manner, with its core being an iterative error state Kalman filter. In some embodiments, the Fast LIO framework uses an iKD-Tree data structure for feature storage and nearest neighbor search. The IMU data first undergoes pre-integration processing, and between two frames of LiDAR data, the pose change of the UAV is predicted by integrating the IMU measurements. This prediction result is used to perform motion compensation on the point cloud of the current frame, eliminating point cloud distortion caused by UAV motion. The compensated point cloud then undergoes feature extraction, primarily extracting planar and edge features. Planar features are identified by calculating the eigenvalues ​​of the local covariance matrix of the points; when the minimum eigenvalue is much smaller than the other two eigenvalues, the region is considered planar. Edge features are extracted in regions where the maximum eigenvalue is much larger than the other two eigenvalues.

[0066] S4: Construct a priori structural map of the underground plant, wherein the priori structural map contains geometric primitive information of the regular structure within the plant.

[0067] Specifically, the raw point cloud data collected above underwent downsampling to reduce the data volume and improve processing efficiency. Then, the RANSAC algorithm was applied to extract geometric primitives from the point cloud. For plane extraction, the algorithm randomly selects three non-collinear points to define a plane, calculates the distances from other points to this plane, counts the number of interior points, and after multiple iterations, selects the plane with the most interior points as the extraction result. Large-area planar structures such as walls, floors, and ceilings can usually be accurately identified and stored in the form of a plane equation of ax + by + cz + d = 0. For line extraction, mainly targeting features such as beam and column edges and corner lines, the algorithm selects two points to define a straight line, and similarly finds the optimal line parameters through RANSAC iteration. All extracted geometric primitives together constitute a priori structure map, which, although sparse, contains the most stable and significant structural features of the factory building.

[0068] S5: Match the features extracted from the current frame with the prior structure map to generate matching constraints;

[0069] The extracted features need to be matched with the map to determine the precise pose of the UAV. This invention innovatively introduces prior structured map constraints on top of standard FastLIO feature matching. The planar features extracted in the current frame are matched not only with the local map maintained by the iKD-Tree from point to surface, but also with planar primitives in the prior map. For example, if a large vertical plane is identified in the current frame, the system will search for walls with similar positions and orientations in the prior map. After a successful match, the average distance from the current planar point cloud to the prior planar equation is calculated as the residual. Similarly, edge features are also matched with line primitives in the prior map.

[0070] S6: Add the matching constraints to the state optimization of the iterative error state Kalman filter to obtain the optimized UAV pose estimation and environment map.

[0071] Specifically, the matching residuals mentioned above, together with the traditional point-to-area and point-to-line residuals, constitute the objective function of the optimization problem. The key innovation lies in assigning a higher weight to the residuals of the prior map matching, typically with a weight ratio of 10:1 between prior constraints and local constraints. This ensures that global localization remains stable even when local feature quality is poor. The optimization problem is solved using the Levenberg-Marquardt algorithm, iteratively updating the UAV pose estimate until the residuals converge to a set threshold.

[0072] The optimized pose is used to update the global map. Newly scanned point clouds are filtered and downsampled before being inserted into the iKD-Tree data structure. The iKD-Tree is an incremental kd-tree that supports efficient point insertion, deletion, and nearest neighbor search, making it particularly suitable for maintaining dynamic maps. Simultaneously, the system continuously monitors for loop closures. When the UAV returns to a previously visited area, loop closures are identified by detecting high matching degrees between current features and historical maps, and global pose graph optimization is performed to eliminate accumulated errors.

[0073] In other embodiments, the method of the present invention also implements equipment status monitoring function based on precise positioning and mapping, including: S7: Real-time segmentation of point cloud clusters representing large equipment based on the optimized environmental map;

[0074] S8: Compare the current position of the device point cloud cluster with the baseline position to detect whether the device has shifted or deformed.

[0075] Specifically, the system performs semantic segmentation on the constructed point cloud map to identify point cloud clusters representing different devices. Taking a generator set as an example, the point cloud belonging to the generator base is segmented through pre-labeling or automatic clustering algorithms. During each inspection, the centroid position and main axis direction of the current generator point cloud cluster are calculated and compared with the baseline data established during the initial inspection. If a positional offset exceeding 5 millimeters or a tilt angle change exceeding 0.1 degrees is detected, the system generates an early warning message. This millimeter-level detection accuracy benefits from the high positioning accuracy brought about by prior map constraints.

[0076] To verify the effectiveness of the method of this invention, a field test was conducted in the underground powerhouse of a pumped storage power station. The powerhouse is 150 meters long, 40 meters wide, and 50 meters high, and houses four generator sets and numerous auxiliary equipment. During the test, a drone flew at a speed of 2 meters per second, completing a full scan of the powerhouse in approximately 30 minutes. Comparative tests showed that the standard Fast LIO algorithm, without the optimized method of this invention, exhibited significant positioning jumps in areas with dense metal equipment, with a maximum error exceeding 3 meters, and produced severe map ghosting in areas with similar structures. However, using the method of this invention, the positioning error was controlled within 10 centimeters throughout the process, and the constructed 3D map was clear and accurate, capable of distinguishing structural details larger than 5 centimeters.

[0077] Upon completion of the task, the generated high-precision 3D point cloud model can be exported in standard formats such as LAS, PLY, or E57, facilitating import into BIM platforms or digital twin systems. Through integration with equipment operation data and maintenance records, comprehensive digital management of the underground powerhouse is achieved. Maintenance personnel can intuitively view equipment status, plan maintenance routes, and simulate modification schemes on the 3D model, significantly improving the power plant's operation and maintenance efficiency and safety.

[0078] In another embodiment, the present invention also provides an optimization system for underground power plants based on unmanned aerial vehicle (UAV) Fast LIO technology, comprising:

[0079] The data acquisition module is used to acquire raw point cloud data and inertial data collected by a drone equipped with a lidar and an inertial measurement unit in an underground factory environment.

[0080] The point cloud preprocessing module is used to filter the original point cloud data to remove interference points caused by reflection from the metal surface.

[0081] The data fusion module is used to fuse preprocessed point cloud data and inertial data using an iterative error state Kalman filter based on the Fast LIO framework.

[0082] The prior map construction module is used to construct a prior structure map containing information on the geometric primitives of the regular structure within the factory building;

[0083] The feature matching module is used to match the features extracted from the current frame with the prior structure map to generate matching constraints;

[0084] The state optimization module is used to add the matching constraints to the state optimization of the iterative error state Kalman filter, and output the optimized UAV pose estimate and environment map.

[0085] In another embodiment, the present invention further includes a computer-readable storage medium storing a computer program that, when executed by a processor, implements the method described above.

[0086] It should be noted that although this embodiment uses an underground powerhouse of a pumped storage power station as an example, the method of this invention is also applicable to other industrial environments with similar characteristics, such as underground substations, subway stations, and large industrial plants. Depending on the specific application scenario, the algorithm parameters, such as the filtering threshold, weight ratio, and feature extraction parameters, can be adaptively adjusted to achieve the best localization and mapping results.

[0087] The above embodiments are only for illustrating the technical concept and features of the present invention, and are intended to enable those skilled in the art to understand the content of the present invention and implement it accordingly. They should not be construed as limiting the scope of protection of the present invention. All equivalent transformations or modifications made in accordance with the spirit and essence of the present invention should be covered within the scope of protection of the present invention.

Claims

1. An optimization method for underground power plants based on Unmanned Aerial Vehicle (UAV) Fast LIO technology, characterized in that, Includes the following steps: S1: Acquire raw point cloud data and inertial data collected by a drone equipped with a lidar and inertial measurement unit in an underground factory environment; S2: Preprocess the original point cloud data to filter out interference points generated by reflection from the metal surface; S3: Based on the Fast LIO framework, feature extraction and matching are performed by fusing preprocessed point cloud data and inertial data through an iterative error state Kalman filter. S4: Construct a priori structural map of the underground plant, wherein the priori structural map contains geometric primitive information of the regular structure within the plant; Planar geometric primitives include the walls, floors, and ceilings of a building; linear geometric primitives include the edges of beams and columns and the corner lines. S5: Match the features extracted from the current frame with the prior structure map to generate matching constraints; The matching process includes: Extract planar features and edge features from the current frame point cloud; Search the prior structure map for geometric primitives corresponding to the features of the current frame; Calculate the residual between the current frame features and the corresponding geometric primitives in the prior map; S6: Add the matching constraints to the state optimization of the iterative error state Kalman filter to obtain the optimized UAV pose estimation and environment map.

2. The method according to claim 1, characterized in that, The preprocessing of the raw point cloud data in step S2 includes: Based on the echo intensity information of lidar, points with abnormally high intensity values ​​that do not conform to the surrounding geometry are identified and filtered out. An adaptive radius filtering algorithm is used to remove spatially sparse outliers; A statistical outlier removal algorithm is applied to remove isolated noise points caused by specular reflection or multipath effects on metal surfaces.

3. The method according to claim 1, characterized in that, The method for constructing the prior structure map in step S4 includes: An initial scan of the underground factory building was performed to obtain initial point cloud data; The RANSAC algorithm is used to extract planar and linear geometric primitives from the initial point cloud data; The extracted geometric primitives are stored in a parametric form, where planes are stored in the form of plane equations and lines are stored in the form of line equations.

4. The method according to claim 1, characterized in that, In step S6, the residual is added as a high-weight constraint term to the objective function of the Levenberg-Marquardt optimization algorithm.

5. The method according to claim 1, characterized in that, Also includes: S7: Based on the optimized environment map, segment point cloud clusters representing large equipment in real time; S8: Compare the current position of the device point cloud cluster with the baseline position to detect whether the device has shifted or deformed.

6. The method according to claim 1, characterized in that, The Fast LIO framework in step S3 uses the iKD-Tree data structure for feature storage and nearest neighbor search, and utilizes IMU pre-integration to perform motion compensation and distortion correction on the lidar point cloud.

7. An optimization system for underground factories based on Unmanned Aerial Vehicle (UAV) Fast LIO technology, characterized in that, include: The data acquisition module is used to acquire raw point cloud data and inertial data collected by a drone equipped with a lidar and an inertial measurement unit in an underground factory environment. The point cloud preprocessing module is used to filter the original point cloud data to remove interference points caused by reflection from the metal surface. The data fusion module is used to fuse preprocessed point cloud data and inertial data using an iterative error state Kalman filter based on the Fast LIO framework. The prior map construction module is used to construct a prior structure map containing information on the geometric primitives of the regular structure within the factory building. The geometric primitives include the planes of walls, floors, and ceilings stored in the form of planar equations, and the straight lines of beam and column edges and corner lines stored in the form of straight line equations. The feature matching module is used to extract planar features and edge features from the current frame point cloud, search for geometric primitives corresponding to the current frame features in the prior structure map, and calculate the residual between the current frame features and the corresponding geometric primitives in the prior map. The state optimization module is used to add the residual as a matching constraint to the state optimization of the iterative error state Kalman filter, and output the optimized UAV pose estimate and environment map.

8. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program that, when executed by a processor, implements the method as described in any one of claims 1 to 6.

Images