Indoor three-dimensional mapping method for unmanned aerial vehicle

Abstract

The invention provides a method for indoor three-dimensional mapping of an unmanned aerial vehicle, which comprises the following steps: converting polar coordinate data into Cartesian coordinate point cloud under a body coordinate system, and performing motion distortion correction on the point cloud based on high-frequency pose data of an inertial measurement unit to obtain corrected point cloud; intercepting the region of interest as a candidate ground point, performing plane fitting on the candidate ground point, and separating the ground point from a non-ground point based on a fitting plane; taking the pre-integration pose of the inertial measurement unit as initial estimation, and performing iterative nearest point matching on a current scanning frame formed by non-ground points and the established local subgraph to obtain an optimized pose of the unmanned aerial vehicle at the current moment; inserting a non-ground point of the current scanning frame into the local sub-graph based on the optimized pose, performing global pose graph optimization when a loop is detected, and outputting a three-dimensional point cloud map and a two-dimensional grid map generated by projection of the non-ground point. And the mapping precision, robustness and practicability of the unmanned aerial vehicle in a complex indoor environment are comprehensively improved.

Description

Technical Field

[0001] This invention relates to the field of environmental perception and SLAM technology for mobile robots, and in particular to a method for indoor 3D mapping of unmanned aerial vehicles. Background Technology

[0002] Currently, SLAM technology is the foundation for UAVs to achieve autonomous navigation. However, the direct application of existing laser SLAM algorithms to UAV platforms faces many challenges. Traditional 2D SLAM algorithms (such as Gmapping) heavily rely on odometry accuracy. Furthermore, the six degrees of freedom motion of UAVs during flight (especially pitch and roll) can cause the 2D LiDAR scanning plane to tilt, resulting in severe ghosting and geometric distortion in the generated map. Secondly, during indoor flight, the LiDAR continuously scans the ground. If the ground point cloud is directly projected onto the 2D grid map, the navigation algorithm may misjudge the ground as impassable obstacles, leading to path planning failure. Finally, in environments with simple geometric features, such as long corridors and open warehouses, feature-matching-based SLAM algorithms are prone to matching errors, resulting in positioning drift or map fragmentation. Therefore, in order to overcome the above-mentioned defects, the present invention provides a method for indoor 3D mapping of unmanned aerial vehicles. Summary of the Invention

[0003] This invention provides a method for indoor 3D mapping of unmanned aerial vehicles (UAVs). By introducing a dedicated processing flow for the six degrees of freedom motion of UAVs, it effectively overcomes three major bottlenecks of traditional laser SLAM algorithms applied to UAV platforms: geometric distortion, ground misidentification, and feature degradation leading to environmental matching failure. First, by fusing high-frequency IMU data for motion distortion correction and ground segmentation, map ghosting and navigation misjudgments caused by aircraft tilt and ground point interference are significantly eliminated, improving the purity and usability of the 2D grid map. Second, by using IMU pre-integration pose as a strong initial estimate for scan matching, the matching success rate and positioning accuracy in low-feature scenarios such as long corridors are greatly improved, enhancing the system's environmental adaptability. Finally, global consistency optimization is achieved through sub-graph fusion and loop closure detection. While outputting a high-precision 3D point cloud map, a 2D occupied grid map that can be directly used for real-time path planning is generated simultaneously, achieving a balance between the richness of perceptual information and navigation computation efficiency. This comprehensively improves the mapping accuracy, robustness, and practicality of UAVs in complex indoor environments.

[0004] This invention provides a method for indoor 3D mapping of unmanned aerial vehicles, comprising: Step 1: Convert the polar coordinate data collected by the lidar into a Cartesian coordinate point cloud in the body coordinate system, and perform motion distortion correction on the point cloud based on the high-frequency pose data of the inertial measurement unit to obtain the corrected point cloud. Step 2: Based on the UAV's flight altitude, extract the region of interest from the corrected point cloud as candidate ground points, and use the random sampling consensus algorithm to perform plane fitting on the candidate ground points, and separate ground points from non-ground points based on the fitted plane; Step 3: Using the pre-integrated pose of the inertial measurement unit as the initial estimate, perform iterative nearest-point matching between the current scan frame composed of non-ground points and the established local sub-graph to obtain the optimized pose of the UAV at the current moment. Step 4: Based on the optimized pose, insert the non-ground points of the current scan frame into the local sub-graph, and optimize the global pose graph when a loop closure is detected, outputting a 3D point cloud map and a 2D raster map generated by projecting the non-ground points.

[0005] Preferably, in a method for indoor 3D mapping of UAVs, step 1 involves converting polar coordinate data acquired by a lidar into a Cartesian coordinate point cloud in the aircraft's body coordinate system, including: Acquire polar coordinate data for each point obtained from the LiDAR scan, where the polar coordinate data includes distance and angle values; Based on the Cartesian coordinate transformation formula, coordinate transformation is performed on the distance and angle values ​​to obtain the projected coordinates of each point on the horizontal plane of the body; Based on the fixed installation height of the lidar in the body coordinate system, the Z-axis coordinate value corresponding to each projected coordinate is determined, and a three-dimensional Cartesian coordinate point cloud in the body coordinate system is generated based on the Z-axis coordinate value corresponding to each projected coordinate.

[0006] Preferably, in a method for indoor 3D mapping of unmanned aerial vehicles, step 1 involves correcting motion distortion of the point cloud based on high-frequency pose data from an inertial measurement unit to obtain a corrected point cloud, including: Acquire high-frequency angular velocity and acceleration data output by the inertial measurement unit within a single frame scan cycle of the lidar; Pre-integration is performed based on high-frequency angular velocity and acceleration data to obtain a continuous time pose sequence within the scanning period with the scanning start time as the reference. Determine the timestamp of the emission time corresponding to each laser point in the point cloud, and use an interpolation strategy to determine the corresponding instantaneous body pose from the continuous time pose sequence based on the timestamp of each laser point; Based on the instantaneous body pose corresponding to each laser point, the current laser point is transformed from the corresponding instantaneous body coordinate system to the body coordinate system at the start of the scan, thereby obtaining the correction point cloud.

[0007] Preferably, in a method for indoor 3D mapping of unmanned aerial vehicles (UAVs), step 2 involves extracting regions of interest from the calibrated point cloud as candidate ground points based on the UAV's flight altitude, including: Obtain the drone's current altitude relative to the ground; Based on the preset vertical tolerance threshold and flight altitude, the vertical altitude range below the UAV body is determined, and based on the Z-axis coordinate range corresponding to the vertical altitude range, all three-dimensional points whose Z coordinate values ​​fall within the Z-axis coordinate range are selected from the calibration point cloud. Candidate ground points are obtained based on the selected set of 3D points.

[0008] Preferably, in a method for indoor 3D mapping of UAVs, step 2 involves using a random sampling consensus algorithm to perform planar fitting on candidate ground points, including: Three non-collinear points are randomly selected from the candidate ground points to form a minimum sample set, and the initial plane model parameters are obtained based on the minimum sample set and the geometric principles of the plane. Determine the vertical distance from all candidate ground points to the initial plane based on the parameters of the initial plane model; Points with a vertical distance less than a preset distance threshold are marked as interior points, and the number of interior points corresponding to the current initial planar model is counted. The process involves iteratively selecting random planes, calculating models, and counting interior points. The initial plane model with the most interior points is selected from all the initial plane models obtained during the iteration process, and the corresponding plane parameters are determined as the final ground fitting plane parameters.

[0009] Preferably, in a method for indoor 3D mapping of UAVs, step 2, separating ground points and non-ground points based on a fitted plane, includes: Obtain the final ground fitting plane parameters determined by the random sampling consensus algorithm, and determine the vertical distance from each point in the corrected point cloud to the final ground fitting plane based on the ground fitting plane parameters; Compare the vertical distance with a preset distance threshold; Points with a vertical distance less than a preset distance threshold are classified as ground points, and points with a vertical distance greater than or equal to the preset distance threshold are classified as non-ground points.

[0010] Preferably, in a method for indoor 3D mapping of UAVs, step 3 involves using the pre-integrated pose of the inertial measurement unit as an initial estimate, and performing iterative nearest-point matching between the current scan frame composed of non-ground points and the established local sub-map to obtain the optimized pose of the UAV at the current moment, including: Define a local search window in the pose space corresponding to the local sub-image, and within the local search window, determine the degree of overlap when the current scan frame and the local sub-image coincide based on different poses based on pose enumeration, and take the position with the highest degree of overlap as the coarse matching pose. Obtain the estimated pose corresponding to the current scanning time, obtained by pre-integration of data from the inertial measurement unit; The coarse matching pose and the predicted pose are fused to generate the optimized initial pose for the iterative closest point algorithm. The optimized initial pose is used as the initial value for transformation to establish the correspondence between the current scanned point cloud and the nearest neighbor points in the local sub-graph, and the sum of squared Euclidean distances between corresponding point pairs is minimized to obtain the optimized pose of the UAV at the current moment.

[0011] Preferably, in a method for indoor 3D mapping of UAVs, step 4, inserting non-ground points of the current scan frame into a local sub-map based on optimized pose, includes: The optimized pose of the UAV at the current moment is obtained based on iterative nearest point matching, and all non-ground points constituting the current scan frame are transformed from the current body coordinate system to the world coordinate system corresponding to the local sub-graph based on the optimized pose. Each non-ground point transformed to the world coordinate system is mapped to the probabilistic raster data structure of the local subgraph, and the probability value of each raster cell being occupied is accumulated based on the update rules of the probabilistic raster data structure. Non-ground points in the current scan frame are inserted into a local sub-graph based on probability values.

[0012] Preferably, in a method for indoor 3D mapping of UAVs, step 4 involves global pose graph optimization upon detecting a loop closure, outputting a 3D point cloud map and a 2D raster map generated from non-ground point projections, including: The branch and bound algorithm is used to match the current scan frame with all historical sub-graphs. When the matching score exceeds the preset confidence threshold, it is determined that a loop closure has been detected. Based on the current frame pose associated with the loop and the successfully matched historical subgraph pose, add a new loop constraint edge to the global pose graph containing all subgraph nodes. The poses of all subgraph nodes in the global pose graph are used as optimization variables, and the front-end measurement constraints and loop closure constraints are used as edges to construct a sparse pose adjustment optimization problem. Solve the sparse pose adjustment optimization problem to obtain the optimal pose of all subgraph nodes after global optimization; The position of each subgraph in the world coordinate system is updated based on the optimal pose of all subgraph nodes after global optimization, and the point clouds of all subgraphs are merged to generate a globally consistent 3D point cloud map.

[0013] Preferably, a method for indoor 3D mapping of UAVs, which fuses all sub-map point clouds to generate a globally consistent 3D point cloud map, includes: Obtain the generated globally consistent 3D point cloud map; Projecting non-ground points in a 3D point cloud map along the height direction onto a specified horizontal plane generates a 2D occupied grid map.

[0014] Compared with the prior art, the beneficial effects of the present invention are as follows: By introducing a dedicated processing flow for the six degrees of freedom motion of UAVs, this method effectively overcomes three major bottlenecks in the application of traditional laser SLAM algorithms on UAV platforms: geometric distortion, ground misidentification, and feature degradation leading to environmental matching failure. First, by fusing high-frequency IMU data for motion distortion correction and ground segmentation, map ghosting and navigation misjudgments caused by aircraft tilt and ground point interference are significantly eliminated, improving the purity and usability of the 2D grid map. Second, by using IMU pre-integrated pose as a strong initial estimate for scan matching, the matching success rate and positioning accuracy in low-feature scenarios such as long corridors are greatly improved, enhancing the system's environmental adaptability. Finally, global consistency optimization is achieved through sub-graph fusion and loop closure detection. While outputting a high-precision 3D point cloud map, a 2D occupied grid map that can be directly used for real-time path planning is generated simultaneously, achieving a balance between the richness of perception information and navigation computation efficiency. This comprehensively improves the mapping accuracy, robustness, and practicality of UAVs in complex indoor environments.

[0015] Other features and advantages of the invention will be set forth in the description which follows, and will be apparent in part from the description, or may be learned by practicing the invention. The objects and other advantages of the invention may be realized and obtained by means of the structures particularly pointed out in this application.

[0016] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings and embodiments. Attached Figure Description

[0017] The accompanying drawings are provided to further illustrate the invention and form part of the specification. They are used in conjunction with embodiments of the invention to explain the invention and do not constitute a limitation thereof. In the drawings: Figure 1 This is a flowchart of a method for indoor 3D mapping of unmanned aerial vehicles (UAVs) according to an embodiment of the present invention; Figure 2 This is a flowchart of step 1 in a method for indoor 3D mapping of a UAV according to an embodiment of the present invention; Figure 3 This is a flowchart of step 2 in a method for indoor 3D mapping of a UAV according to an embodiment of the present invention. Detailed Implementation

[0018] The preferred embodiments of the present invention will be described below with reference to the accompanying drawings. It should be understood that the preferred embodiments described herein are for illustration and explanation only and are not intended to limit the present invention.

[0019] Example 1: This example provides a method for indoor 3D mapping of unmanned aerial vehicles (UAVs), such as... Figure 1 As shown, it includes: Step 1: Convert the polar coordinate data collected by the lidar into a Cartesian coordinate point cloud in the body coordinate system, and perform motion distortion correction on the point cloud based on the high-frequency pose data of the inertial measurement unit to obtain the corrected point cloud. Step 2: Based on the UAV's flight altitude, extract the region of interest from the corrected point cloud as candidate ground points, and use the random sampling consensus algorithm to perform plane fitting on the candidate ground points, and separate ground points from non-ground points based on the fitted plane; Step 3: Using the pre-integrated pose of the inertial measurement unit as the initial estimate, perform iterative nearest-point matching between the current scan frame composed of non-ground points and the established local sub-graph to obtain the optimized pose of the UAV at the current moment. Step 4: Based on the optimized pose, insert the non-ground points of the current scan frame into the local sub-graph, and optimize the global pose graph when a loop closure is detected, outputting a 3D point cloud map and a 2D raster map generated by projecting the non-ground points.

[0020] In this embodiment, motion distortion correction refers to the process of using high-frequency pose data from the inertial measurement unit to correct the point cloud distortion caused by the movement of the UAV during a single-frame scan of the lidar.

[0021] In this embodiment, the corrected point cloud refers to the three-dimensional point cloud data obtained after coordinate transformation and motion distortion correction, which is unified in the body coordinate system at the start of the scan.

[0022] In this embodiment, candidate ground points refer to a set of three-dimensional points that may belong to the ground, extracted from the calibration point cloud based on the UAV's flight altitude and located within a specific altitude range below the aircraft.

[0023] In this embodiment, non-ground points refer to points in the corrected point cloud that are classified as not belonging to the ground by plane fitting and distance threshold determination, and usually represent obstacles or environmental structures.

[0024] In this embodiment, pre-integrated pose refers to the estimation of the relative pose change of the UAV obtained by integrating the angular velocity and acceleration data of the inertial measurement unit between adjacent scanning times of the lidar.

[0025] In this embodiment, the local sub-map refers to an intermediate map representation that is constructed by accumulating multiple consecutive frames of laser scanning data in a local coordinate system and is used for real-time front-end matching and local map maintenance.

[0026] In this embodiment, the historical subgraph refers to the subgraph that has been constructed and added to the global pose graph for backend optimization and management.

[0027] In this embodiment, global pose graph optimization refers to the process of jointly optimizing the poses and loop closure relationships of all subgraphs when loop closure constraints are detected, in order to eliminate accumulated errors and achieve a globally consistent map.

[0028] In this embodiment, the two-dimensional occupancy grid map refers to a two-dimensional map that projects non-ground points in a three-dimensional point cloud onto a specified horizontal plane and uses grid cell probability values ​​to represent the occupancy status of obstacles.

[0029] The working principle of the above technical solution is as follows: First, the original point cloud is preprocessed and distortion corrected. Then, by introducing a pipeline of "altitude screening -> RANSAC fitting -> ground filtering" tailored to the characteristics of UAVs, the problem of ground being misidentified as obstacles is effectively solved. Next, by deeply fusing IMU pre-integrated data as a strong prior for ICP matching, the short-term accuracy of inertial navigation is used to constrain the search space of laser matching, significantly improving the matching success rate and system robustness in feature degradation environments such as long corridors. Finally, loop closure detection and global optimization are used to eliminate accumulated errors, and a 3D point cloud map that can be used for high-level cognition and a 2D grid map for real-time path planning are output simultaneously, perfectly meeting the requirements of the ROS2 navigation framework.

[0030] The beneficial effects of the above technical solution are as follows: By introducing a dedicated processing flow for the six degrees of freedom motion of UAVs, it effectively overcomes the three major bottlenecks of traditional laser SLAM algorithms when applied to UAV platforms: geometric distortion, ground misidentification, and feature degradation leading to environmental matching failure. First, by fusing high-frequency IMU data for motion distortion correction and ground segmentation, it significantly eliminates map ghosting and navigation misjudgment caused by aircraft tilt and ground point interference, improving the purity and usability of the 2D grid map. Second, by using IMU pre-integration pose as a strong initial estimate for scanning matching, it greatly improves the matching success rate and positioning accuracy in low-feature scenarios such as long corridors, enhancing the system's environmental adaptability. Finally, by achieving global consistency optimization through sub-graph fusion and loop closure detection, it simultaneously generates a 2D occupied grid map that can be directly used for real-time path planning while outputting a high-precision 3D point cloud map, achieving a balance between the richness of perception information and navigation computation efficiency, and comprehensively improving the mapping accuracy, robustness, and practicality of UAVs in complex indoor environments.

[0031] Example 2: Based on Example 1, this example provides a method for indoor 3D mapping of unmanned aerial vehicles (UAVs), such as... Figure 2 As shown, in step 1, the polar coordinate data collected by the lidar is converted into a Cartesian coordinate point cloud in the body coordinate system, including: Step 101: Obtain the polar coordinate data of each point obtained by the LiDAR scan, wherein the polar coordinate data includes distance value and angle value; Step 102: Perform coordinate transformation on the distance and angle values ​​based on the Cartesian coordinate transformation formula to obtain the projected coordinates of each point on the horizontal plane of the body; Step 103: Based on the fixed installation height of the lidar in the body coordinate system, determine the Z-axis coordinate value corresponding to each projected coordinate, and generate a three-dimensional Cartesian coordinate point cloud in the body coordinate system based on the Z-axis coordinate value corresponding to each projected coordinate.

[0032] In this embodiment, when the distance value and angle value are r and v respectively, the distance value and angle value are transformed according to the Cartesian coordinate transformation formula, specifically as follows: x = r × cosv, y = r × sinθ.

[0033] In this embodiment, the projected coordinates refer to two-dimensional planar coordinates that are calculated using the Cartesian coordinate transformation formula and contain only X-axis and Y-axis components, without including height information.

[0034] The beneficial effects of the above technical solution are as follows: by standardizing and converting the raw polar coordinate data collected by the lidar into a three-dimensional Cartesian coordinate point cloud in the body coordinate system, a unified and accurate spatial data foundation is provided for all subsequent processing. This not only standardizes the data format and eliminates the differences between the raw sensor data and the algorithm processing interface, but more importantly, by combining the fixed installation height of the lidar to give the point cloud vertical dimension information, the three-dimensional structural features of the environment are completely preserved. This provides accurate input for subsequent processing such as motion distortion correction and ground segmentation, and is the primary key link to ensure the accuracy and consistency of the entire mapping process.

[0035] Example 3: Based on Example 1, this example provides a method for indoor 3D mapping of UAVs. In step 1, motion distortion correction is performed on the point cloud based on the high-frequency pose data of the inertial measurement unit to obtain a corrected point cloud, including: Acquire high-frequency angular velocity and acceleration data output by the inertial measurement unit within a single frame scan cycle of the lidar; Pre-integration is performed based on high-frequency angular velocity and acceleration data to obtain a continuous time pose sequence within the scanning period with the scanning start time as the reference. Determine the timestamp of the emission time corresponding to each laser point in the point cloud, and use an interpolation strategy to determine the corresponding instantaneous body pose from the continuous time pose sequence based on the timestamp of each laser point; Based on the instantaneous body pose corresponding to each laser point, the current laser point is transformed from the corresponding instantaneous body coordinate system to the body coordinate system at the start of the scan, thereby obtaining the correction point cloud.

[0036] In this embodiment, the continuous time pose sequence refers to a series of body pose data that change continuously over time within the single-frame scanning time range of the lidar, obtained by pre-integrating high-frequency angular velocity and acceleration data.

[0037] In this embodiment, the timestamp refers to the precise emission time corresponding to each laser point in the lidar point cloud.

[0038] In this embodiment, instantaneous aircraft pose refers to the precise position and attitude of the UAV at a specific moment, obtained from a continuous time pose sequence based on the timestamp of the laser point using an interpolation method.

[0039] The beneficial effects of the above technical solution are: by using high-frequency pose data to correct the point cloud distortion caused by the movement of the UAV during the scanning process point by point, motion blur is effectively eliminated, the spatial consistency and geometric accuracy of the point cloud are significantly improved, and a reliable data foundation is laid for subsequent accurate matching and map construction.

[0040] Example 4: Based on Example 1, this example provides a method for indoor 3D mapping of unmanned aerial vehicles (UAVs), such as... Figure 3 As shown, in step 2, the region of interest is extracted from the corrected point cloud as candidate ground points based on the UAV's flight altitude, including: Step 201: Obtain the drone's current altitude relative to the ground; Step 202: Based on the preset vertical tolerance threshold and flight altitude, determine the vertical height range below the UAV body, and based on the Z-axis coordinate range corresponding to the vertical height range, filter out all three-dimensional points whose Z coordinate values ​​fall within the Z-axis coordinate range from the calibration point cloud. Step 203: Obtain candidate ground points based on the selected set of 3D points.

[0041] In this embodiment, the vertical tolerance threshold refers to a preset vertical distance value, which, together with the flight altitude, defines an upper and lower boundary interval to determine whether the point cloud is within a reasonable altitude range that may belong to the ground.

[0042] The beneficial effects of the above technical solution are: by quickly filtering out the point cloud subset most likely to contain the ground in the vertical direction based on real-time flight altitude information, the data processing scale of the subsequent plane fitting algorithm is greatly reduced, the overall efficiency of ground segmentation is effectively improved, and accurate input is provided for accurately separating passable areas from obstacles.

[0043] Example 5: Based on Example 1, this example provides a method for indoor 3D mapping of UAVs. In step 2, a random sampling consensus algorithm is used to perform planar fitting on candidate ground points, including: Three non-collinear points are randomly selected from the candidate ground points to form a minimum sample set, and the initial plane model parameters are obtained based on the minimum sample set and the geometric principles of the plane. Determine the vertical distance from all candidate ground points to the initial plane based on the parameters of the initial plane model; Points with a vertical distance less than a preset distance threshold are marked as interior points, and the number of interior points corresponding to the current initial planar model is counted. The process involves iteratively selecting random planes, calculating models, and counting interior points. The initial plane model with the most interior points is selected from all the initial plane models obtained during the iteration process, and the corresponding plane parameters are determined as the final ground fitting plane parameters.

[0044] In this embodiment, the minimum sample set refers to the minimum number of points (three non-collinear points) randomly selected from candidate ground points for computing an initial planar model.

[0045] In this embodiment, the initial plane model parameters refer to the coefficients of a temporary plane equation directly calculated based on the minimum sample set and using plane geometry principles.

[0046] In this embodiment, an interior point refers to a candidate point that is less than a preset distance threshold from the current initial plane in each model evaluation and is therefore considered to conform to the plane model.

[0047] In this embodiment, the ground fitting plane parameters refer to the final plane equation coefficients corresponding to the plane model with the most interior points selected after multiple iterative comparisons.

[0048] The beneficial effects of the above technical solution are: by using the random sampling consensus algorithm to robustly fit the ground plane model from the candidate set that may contain noise and outliers, it effectively overcomes the influence of noise interference and local unevenness in discrete point clouds, can accurately identify large-scale ground structures, and provides an accurate geometric judgment benchmark for subsequent reliable separation of ground and non-ground points.

[0049] Example 6: Based on Example 1, this example provides a method for indoor 3D mapping of UAVs. In step 2, ground points and non-ground points are separated based on a fitted plane, including: Obtain the final ground fitting plane parameters determined by the random sampling consensus algorithm, and determine the vertical distance from each point in the corrected point cloud to the final ground fitting plane based on the ground fitting plane parameters; Compare the vertical distance with a preset distance threshold; Points with a vertical distance less than a preset distance threshold are classified as ground points, and points with a vertical distance greater than or equal to the preset distance threshold are classified as non-ground points.

[0050] In this embodiment, the preset distance threshold refers to a pre-set vertical distance threshold used to determine whether a point belongs to the fitted ground.

[0051] The beneficial effects of the above technical solution are: by using a precise planar model and distance criteria, the point cloud can be clearly distinguished into passable ground and real obstacles, effectively eliminating the interference of ground points on the navigation map and directly improving the purity of the subsequently constructed map and the reliability of path planning.

[0052] Example 7: Based on Example 1, this example provides a method for indoor 3D mapping of UAVs. In step 3, the pre-integrated pose of the inertial measurement unit is used as the initial estimate. Iterative nearest-point matching is performed between the current scan frame composed of non-ground points and the established local sub-map to obtain the optimized pose of the UAV at the current moment, including: Define a local search window in the pose space corresponding to the local sub-image, and within the local search window, determine the degree of overlap when the current scan frame and the local sub-image coincide based on different poses based on pose enumeration, and take the position with the highest degree of overlap as the coarse matching pose. Obtain the estimated pose corresponding to the current scanning time, obtained by pre-integration of data from the inertial measurement unit; The coarse matching pose and the predicted pose are fused to generate the optimized initial pose for the iterative closest point algorithm. The optimized initial pose is used as the initial value for transformation to establish the correspondence between the current scanned point cloud and the nearest neighbor points in the local sub-graph, and the sum of squared Euclidean distances between corresponding point pairs is minimized to obtain the optimized pose of the UAV at the current moment.

[0053] In this embodiment, the pre-integrated pose of the inertial measurement unit (IMU) is used as the initial estimate for iterative nearest-neighbor matching. Specifically, to improve matching accuracy in feature degradation environments, this invention improves the front-end matching strategy of Cartographer. First, Correlation Scan Matching (CSM) is performed as a coarse match, enumerating poses within a local search window to find the optimal initial pose. Then, the ICP algorithm is introduced for fine matching. Traditional ICP is prone to getting trapped in local optima. This invention uses the pre-integrated pose of the IMU as the initial value for ICP iteration. Since the IMU has extremely high short-term accuracy, this provides ICP with a starting point very close to the true value, thereby minimizing the error between the current scanned point cloud and the nearest neighbor point on the map, significantly improving the matching success rate.

[0054] In this embodiment, the local search window refers to a limited range within the pose parameter space corresponding to the local subgraph, defined for the purpose of initial pose search.

[0055] In this embodiment, coarse matching pose refers to the candidate pose with the highest overlap degree selected by enumerating and evaluating the overlap degree between the scan and the sub-image under different poses within a local search window.

[0056] In this embodiment, the estimated pose refers to the UAV pose estimate corresponding to the current laser scanning moment, obtained by pre-integration processing of inertial measurement unit data.

[0057] In this embodiment, optimizing the initial pose refers to the pose obtained by fusing information from the coarse matching pose and the estimated pose, which is used as the starting point for precise registration of the iterative nearest point algorithm.

[0058] In this embodiment, the correspondence refers to the pairing connection between a point in the currently scanned point cloud and its nearest neighbor in the local subgraph, established based on the current transform estimate during the iterative nearest point algorithm process.

[0059] The beneficial effects of the above technical solution are: by integrating two types of pose estimation, geometric coarse matching and inertial pre-integration, it provides a high-precision and strongly constrained initial transformation for fine point cloud registration, effectively avoiding iterative optimization from getting stuck in local optima, and significantly improving the matching success rate and the accuracy of final pose estimation in feature-sparse or repetitive scenarios.

[0060] Example 8: Based on Example 1, this example provides a method for indoor 3D mapping of UAVs. In step 4, non-ground points of the current scan frame are inserted into a local sub-map based on optimized pose, including: The optimized pose of the UAV at the current moment is obtained based on iterative nearest point matching, and all non-ground points constituting the current scan frame are transformed from the current body coordinate system to the world coordinate system corresponding to the local sub-graph based on the optimized pose. Each non-ground point transformed to the world coordinate system is mapped to the probabilistic raster data structure of the local subgraph, and the probability value of each raster cell being occupied is accumulated based on the update rules of the probabilistic raster data structure. Non-ground points in the current scan frame are inserted into a local sub-graph based on probability values.

[0061] In this embodiment, the probabilistic raster data structure refers to a data organization method that discretizes the local subgraph space into regular raster cells and uses probability values ​​to represent the probability that each cell is occupied by an obstacle.

[0062] In this embodiment, the probability value of a grid cell being occupied refers to the confidence value of the presence of an obstacle in each grid cell, which is stored in the grid cell and calculated by accumulating multiple frames of observation data and updating according to specific rules.

[0063] The beneficial effects of the above technical solution are: by using precise optimized pose to integrate the currently scanned obstacle information into the local map, and by dynamically maintaining the occupancy state of the environmental structure through a probability update mechanism, not only is the real-time performance and consistency of the map guaranteed, but the robustness of the map to dynamic changes or noise interference is also significantly improved, providing a reliable environmental model for subsequent real-time navigation and path planning.

[0064] Example 9: Based on Example 1, this example provides a method for indoor 3D mapping of UAVs. In step 4, when a loop closure is detected, global pose graph optimization is performed, and a 3D point cloud map and a 2D raster map generated by projecting non-ground points are output, including: The branch and bound algorithm is used to match the current scan frame with all historical sub-graphs. When the matching score exceeds the preset confidence threshold, it is determined that a loop closure has been detected. Based on the current frame pose associated with the loop and the successfully matched historical subgraph pose, add a new loop constraint edge to the global pose graph containing all subgraph nodes. The poses of all subgraph nodes in the global pose graph are used as optimization variables, and the front-end measurement constraints and loop closure constraints are used as edges to construct a sparse pose adjustment optimization problem. Solve the sparse pose adjustment optimization problem to obtain the optimal pose of all subgraph nodes after global optimization; The position of each subgraph in the world coordinate system is updated based on the optimal pose of all subgraph nodes after global optimization, and the point clouds of all subgraphs are merged to generate a globally consistent 3D point cloud map.

[0065] In this embodiment, all subgraph nodes include the currently constructed local subgraph and all historical subgraphs.

[0066] In this embodiment, global pose graph optimization is performed upon detecting loop closures. Specifically, the map construction and loop closure detection process is as follows: After the subgraph is constructed, it is added to the backend pose graph. The system continuously runs a branch and bound algorithm in the background to detect loop closures. Once a high-confidence match is detected, a loop closure constraint is constructed. Subsequently, a sparse pose adjustment problem is constructed to uniformly optimize the poses of all subgraphs, distributing the accumulated error across the entire trajectory, eliminating map ghosting, and achieving loop closure. Finally, the point cloud positions of all subgraphs are updated based on the optimized poses, and the points are fused to generate a globally consistent 3D point cloud map.

[0067] In this embodiment, the loop constraint edge refers to a spatial constraint relationship added to the global pose graph when a loop is detected, connecting the current pose node and the historical pose node, and used to indicate that the two should be at the same position.

[0068] In this embodiment, the global pose graph refers to a graph data structure that includes all subgraph nodes as vertices and relative pose transformations and loop constraints between adjacent frames as edges.

[0069] In this embodiment, the sparse pose adjustment optimization problem refers to a large-scale nonlinear least squares problem that is constructed with front-end and back-end measurements as constraints and the poses of all nodes in the graph as optimization variables, and can be solved efficiently by taking advantage of the sparse properties of its matrix.

[0070] In this embodiment, the optimal pose of all subgraph nodes refers to the final position and pose of each subgraph in the world coordinate system that minimizes all constraint errors, obtained by solving the global optimization problem.

[0071] The beneficial effects of the above technical solution are as follows: by introducing global map optimization when loop closure is detected, the positioning drift and map misalignment caused by the accumulation of front-end odometer are effectively corrected, map ghosting is eliminated, and the geometric consistency of the global map is ensured. At the same time, based on the optimized high-precision 3D map, the method automatically generates a 2D map for real-time navigation, realizing a closed loop from perception to planning, and greatly improving the accuracy and practical value of the mapping system.

[0072] Example 10: Based on Example 9, this example provides a method for indoor 3D mapping of UAVs, fusing all sub-map point clouds to generate a globally consistent 3D point cloud map, including: Obtain the generated globally consistent 3D point cloud map; Projecting non-ground points in a 3D point cloud map along the height direction onto a specified horizontal plane generates a 2D occupied grid map.

[0073] In this embodiment, generating a two-dimensional occupied grid map specifically includes: while constructing a three-dimensional point cloud map, the present invention generates a two-dimensional grid map in real time for navigation use through projection slicing technology. Specifically, non-ground points in the globally consistent three-dimensional point cloud map are projected along the height direction to a specified horizontal plane (such as the ground plane), thereby generating a two-dimensional occupied grid map that can be used for real-time path planning in navigation frameworks such as Navigation2.

[0074] In this embodiment, the specified horizontal plane refers to a pre-selected reference plane used for vertical projection to generate a two-dimensional map, typically the ground plane.

[0075] The beneficial effects of the above technical solution are: by fusing the point clouds of each sub-map based on the optimized global pose, a precise and consistent three-dimensional environment model is generated, and it is directly converted into a two-dimensional navigation map focused on obstacle information, thus achieving a seamless connection from rich perception to efficient planning.

[0076] Obviously, those skilled in the art can make various modifications and variations to this invention without departing from its spirit and scope. Therefore, if these modifications and variations fall within the scope of the claims of this invention and their equivalents, this invention also intends to include these modifications and variations.

Claims

1. A method for indoor 3D mapping of unmanned aerial vehicles, characterized in that, include: Step 1: Convert the polar coordinate data collected by the lidar into a Cartesian coordinate point cloud in the body coordinate system, and perform motion distortion correction on the point cloud based on the high-frequency pose data of the inertial measurement unit to obtain the corrected point cloud. Step 2: Based on the UAV's flight altitude, extract the region of interest from the corrected point cloud as candidate ground points, and use the random sampling consensus algorithm to perform plane fitting on the candidate ground points, and separate ground points from non-ground points based on the fitted plane; Step 3: Using the pre-integrated pose of the inertial measurement unit as the initial estimate, perform iterative nearest-point matching between the current scan frame composed of non-ground points and the established local sub-graph to obtain the optimized pose of the UAV at the current moment. Step 4: Based on the optimized pose, insert the non-ground points of the current scan frame into the local sub-graph, and optimize the global pose graph when a loop closure is detected, outputting a 3D point cloud map and a 2D raster map generated by projecting the non-ground points.

2. The method for indoor 3D mapping of unmanned aerial vehicles according to claim 1, characterized in that, In step 1, the polar coordinate data collected by the lidar is converted into a Cartesian coordinate point cloud in the body coordinate system, including: Acquire polar coordinate data for each point obtained from the LiDAR scan, where the polar coordinate data includes distance and angle values; Based on the Cartesian coordinate transformation formula, coordinate transformation is performed on the distance and angle values ​​to obtain the projected coordinates of each point on the horizontal plane of the body; Based on the fixed installation height of the lidar in the body coordinate system, the Z-axis coordinate value corresponding to each projected coordinate is determined, and a three-dimensional Cartesian coordinate point cloud in the body coordinate system is generated based on the Z-axis coordinate value corresponding to each projected coordinate.

3. The method for indoor 3D mapping of unmanned aerial vehicles according to claim 1, characterized in that, In step 1, motion distortion correction is performed on the point cloud based on the high-frequency pose data of the inertial measurement unit to obtain the corrected point cloud, including: Acquire high-frequency angular velocity and acceleration data output by the inertial measurement unit within a single frame scan cycle of the lidar; Pre-integration is performed based on high-frequency angular velocity and acceleration data to obtain a continuous time pose sequence within the scanning period with the scanning start time as the reference. Determine the timestamp of the emission time corresponding to each laser point in the point cloud, and use an interpolation strategy to determine the corresponding instantaneous body pose from the continuous time pose sequence based on the timestamp of each laser point; Based on the instantaneous body pose corresponding to each laser point, the current laser point is transformed from the corresponding instantaneous body coordinate system to the body coordinate system at the start of the scan, thereby obtaining the correction point cloud.

4. The method for indoor 3D mapping of unmanned aerial vehicles according to claim 1, characterized in that, In step 2, regions of interest are extracted from the corrected point cloud based on the UAV's flight altitude as candidate ground points, including: Obtain the drone's current altitude relative to the ground; Based on the preset vertical tolerance threshold and flight altitude, the vertical altitude range below the UAV body is determined, and based on the Z-axis coordinate range corresponding to the vertical altitude range, all three-dimensional points whose Z coordinate values ​​fall within the Z-axis coordinate range are selected from the calibration point cloud. Candidate ground points are obtained based on the selected set of 3D points.

5. The method for indoor 3D mapping of unmanned aerial vehicles according to claim 1, characterized in that, In step 2, the random sampling consensus algorithm is used to perform plane fitting on the candidate ground points, including: Three non-collinear points are randomly selected from the candidate ground points to form a minimum sample set, and the initial plane model parameters are obtained based on the minimum sample set and the geometric principles of the plane. Determine the vertical distance from all candidate ground points to the initial plane based on the parameters of the initial plane model; Points with a vertical distance less than a preset distance threshold are marked as interior points, and the number of interior points corresponding to the current initial planar model is counted. The process involves iteratively selecting random planes, calculating models, and counting interior points. The initial plane model with the most interior points is selected from all the initial plane models obtained during the iteration process, and the corresponding plane parameters are determined as the final ground fitting plane parameters.

6. A method for indoor 3D mapping of unmanned aerial vehicles according to claim 1, characterized in that, Step 2 involves separating ground points from non-ground points based on the fitted plane, including: Obtain the final ground fitting plane parameters determined by the random sampling consensus algorithm, and determine the vertical distance from each point in the corrected point cloud to the final ground fitting plane based on the ground fitting plane parameters; Compare the vertical distance with a preset distance threshold; Points with a vertical distance less than a preset distance threshold are classified as ground points, and points with a vertical distance greater than or equal to the preset distance threshold are classified as non-ground points.

7. The method for indoor 3D mapping of unmanned aerial vehicles according to claim 1, characterized in that, In step 3, the pre-integrated pose of the inertial measurement unit is used as the initial estimate. Iterative nearest-point matching is performed between the current scan frame composed of non-ground points and the established local sub-graph to obtain the optimized pose of the UAV at the current moment, including: Define a local search window in the pose space corresponding to the local sub-image, and within the local search window, determine the degree of overlap when the current scan frame and the local sub-image coincide based on different poses, based on pose enumeration, and take the position with the highest degree of overlap as the coarse matching pose. Obtain the estimated pose corresponding to the current scanning time, obtained by pre-integration of data from the inertial measurement unit; The coarse matching pose and the predicted pose are fused to generate the optimized initial pose for the iterative closest point algorithm. The optimized initial pose is used as the initial value for transformation to establish the correspondence between the current scanned point cloud and the nearest neighbor points in the local sub-graph, and the sum of squared Euclidean distances between corresponding point pairs is minimized to obtain the optimized pose of the UAV at the current moment.

8. A method for indoor 3D mapping of unmanned aerial vehicles according to claim 1, characterized in that, In step 4, non-ground points of the current scan frame are inserted into the local sub-map based on the optimized pose, including: The optimized pose of the UAV at the current moment is obtained based on iterative nearest point matching, and all non-ground points constituting the current scan frame are transformed from the current body coordinate system to the world coordinate system corresponding to the local sub-graph based on the optimized pose. Each non-ground point transformed to the world coordinate system is mapped to the probabilistic raster data structure of the local subgraph, and the probability value of each raster cell being occupied is accumulated based on the update rules of the probabilistic raster data structure. Non-ground points in the current scan frame are inserted into a local sub-graph based on probability values.

9. A method for indoor 3D mapping of unmanned aerial vehicles according to claim 1, characterized in that, In step 4, upon detecting a loop closure, global pose graph optimization is performed, outputting a 3D point cloud map and a 2D raster map generated from the projection of non-ground points, including: The branch and bound algorithm is used to match the current scan frame with all historical sub-graphs. When the matching score exceeds the preset confidence threshold, it is determined that a loop closure has been detected. Based on the current frame pose associated with the loop and the successfully matched historical subgraph pose, add a new loop constraint edge to the global pose graph containing all subgraph nodes. The poses of all subgraph nodes in the global pose graph are used as optimization variables, and the front-end measurement constraints and loop closure constraints are used as edges to construct a sparse pose adjustment optimization problem. Solve the sparse pose adjustment optimization problem to obtain the optimal pose of all subgraph nodes after global optimization; The position of each subgraph in the world coordinate system is updated based on the optimal pose of all subgraph nodes after global optimization, and the point clouds of all subgraphs are merged to generate a globally consistent 3D point cloud map.

10. A method for indoor 3D mapping of unmanned aerial vehicles according to claim 9, characterized in that, Merge all sub-map point clouds to generate a globally consistent 3D point cloud map, including: Obtain the generated globally consistent 3D point cloud map; Projecting non-ground points in a 3D point cloud map along the height direction onto a specified horizontal plane generates a 2D occupied grid map.

Images