Closed space autonomous exploration three-dimensional path planning method

Abstract

The invention provides a closed space autonomous exploration three-dimensional path planning method, and belongs to the field of unmanned aerial vehicle path planning. The method specifically comprises the following steps: firstly, in a closed pipeline environment, mapping point cloud data collected at a current t moment to obtain a local voxel map; then principal component analysis is carried out to determine the main direction and each branch of the flight of the unmanned aerial vehicle; the unmanned aerial vehicle explores in all directions, and track points in branch directions are stored in an exploration queue; and continuously and repeatedly exploring the main direction of the point cloud data collected at the t + 1 moment, and storing the track points in each branch direction. When the main direction explored at a certain moment is impassable, selecting a track point with the highest priority from the exploration queue, exploring along the corresponding branch, and deleting the track point; and the next track point with the highest priority is repeatedly selected for exploration until no track point exists in the exploration queue, and a complete detection path is formed. According to the invention, the exploration efficiency and completeness are improved.

Description

Technical Field

[0001] This invention belongs to the field of unmanned aerial vehicle (UAV) path planning, specifically relating to a three-dimensional path planning method for autonomous exploration in enclosed spaces. Background Technology

[0002] In recent years, drones have been widely used in surveying, environmental protection, and rescue fields due to their unique data acquisition and material transportation capabilities. However, in most operational scenarios, they are still under manual operation, lacking sufficient autonomous operation capabilities. As a key component of drone autonomous surveying capabilities, autonomous exploration has received widespread attention, leading to the emergence of many excellent autonomous exploration algorithms.

[0003] Existing exploration methods can explore the environment through frontier or sampling viewpoints, but many problems remain to be solved. In particular, 3D modeling of closed and complex environments (such as large factories, underground tunnels, and underground mines) is the foundation for achieving autonomous navigation, safety inspection, and spatial analysis. Such scenarios are characterized by complex structures (including narrow passages, varying heights, and hidden cavities) and weak global positioning signals (such as the absence of GPS). Furthermore, due to the unknown environmental information, extremely high requirements are placed on the coverage integrity, efficiency, and accuracy of autonomous mapping systems.

[0004] In the prior art, such as in document 1: application number 202210014097.6, publication number CN114355981A, a method and system for autonomous exploration and mapping of a quadcopter UAV improves path planning efficiency by constructing global and local topological maps.

[0005] Reference 2: B. Zhou, Y. Zhang, X. Chen, and S. Shen, “FUEL: Fast UAV exploration using incremental frontier structure and hierarchical planning,” IEEE Robot. Autom. Lett., vol. 6, no. 2, pp. 779–786, Apr. 2021. This paper significantly improves the autonomous mapping efficiency of UAVs through a hierarchical planning strategy.

[0006] However, the lidar mapping method used in the above literature has obvious limitations:

[0007] 1) Focusing solely on exploring passable areas rather than environmental exploration: Most viewpoint-based algorithms prioritize minimizing exploration time or trajectory length, employing a greedy strategy to select the viewpoint with the highest information gain. This can easily lead to drones prematurely leaving structurally complex areas (such as intersections of multiple alleyways or blind spots behind equipment), creating blind spots in the exploration.

[0008] 2) High computational complexity: Another type of method based on frontier exploration can quickly explore the entire environment by searching for frontiers and generating exploration sequences. However, the process of finding and describing frontiers is too random, resulting in high computational costs.

[0009] 3) Failure to consider the three-dimensional flight characteristics of UAVs: Some existing algorithms are often based on the two-dimensional autonomous exploration algorithms of ground unmanned vehicles, without combining the flight characteristics of UAVs for path optimization, resulting in path redundancy and insufficient safety.

[0010] 4) Existing full-coverage mapping algorithms are mostly designed for known environments (such as preset paths for farmland inspections), making it difficult to adapt to the autonomous exploration needs of unknown and complex environments. Furthermore, autonomous exploration algorithms often aim for "rapid coverage of unknown areas," failing to strictly guarantee the consistency of point cloud resolution, resulting in uneven mapping accuracy. Therefore, there is an urgent need for a lidar mapping method that can autonomously plan paths, dynamically optimize observation point positions, and ensure global coverage and resolution compliance in unknown and complex environments. Summary of the Invention

[0011] To address the challenge of enabling fully autonomous 3D exploration and high-precision mapping of unmanned aerial vehicles (UAVs) in enclosed underground environments (such as mines, tunnels, and underground facilities) lacking GPS signals and prior information, this invention provides a 3D path planning method for autonomous exploration in enclosed spaces. By dynamically generating optimized observation points, implementing global path planning, and optimizing trajectory smoothing within the enclosed environment, this method achieves efficient, complete, and high-precision mapping of unknown environments.

[0012] The specific steps of the autonomous three-dimensional path planning method for exploring enclosed spaces are as follows:

[0013] Step 1: In a closed pipeline environment without GPS, collect point cloud and inertial data in real time using a tightly coupled LiDAR and IMU solution;

[0014] The drone, equipped with a lidar, can measure the three-dimensional coordinates of tunnel walls, obstacles, and other objects in a closed environment without GPS by emitting laser beams, generating point cloud data.

[0015] Step 2: For the point cloud data collected at time t, the Euclidean Symbolic Distance Field (ESDF) and octree map structure are combined to map the point cloud data onto three-dimensional cube cells to form a local voxel map.

[0016] First, the point cloud data collected at time t is mapped to a three-dimensional cube element of the closed pipe environment, with each element being a voxel; the initial value of t is 1.

[0017] Then, the voxel data is managed using the hierarchical tree structure of the octree map, and the calculations are performed for each voxel. Occupancy probability This indicates the probability that the voxel is occupied by an obstacle.

[0018]

[0019] in This represents all observation point cloud data from time 1 to time t.

[0020] When the probability of a voxel being occupied by an obstacle exceeds a set threshold, the voxel is marked as an obstacle; otherwise, the distance from each voxel to the nearest obstacle is calculated using the Euclidean Symbolic Distance Field (ESDF) and stored to determine whether the drone can pass.

[0021]

[0022] in For the set of elements representing obstacles, Voxel representation Inside the obstacle (negative value) or outside the obstacle (positive value).

[0023] Step 3: On the local voxel map, perform principal component analysis using the current point cloud to explore the main direction of the UAV's flight and determine whether there are branches.

[0024] Specifically: First, for the current point cloud set Perform principal component analysis and calculate the covariance matrix:

[0025]

[0026] in The center of mass of the point cloud.

[0027] Then, calculate the covariance matrix. eigenvalues, largest eigenvalue Corresponding feature vector That is, the main direction of the current point cloud distribution. .

[0028] When the largest eigenvalue with the second largest eigenvalue ratio When the value is close to 1 or less than the threshold, it indicates that the tunnel has multiple extension directions, i.e., branches.

[0029] Step 4: Optimize the main direction. The UAV explores along the main direction and each branch direction separately to obtain the center candidate point in each direction, and further searches for waypoints, storing the waypoints of each branch direction into the exploration queue. .

[0030] Optimization refers to: optimizing the current estimated main direction. Compared with the historical average direction obtained based on recent trajectory smoothing By performing weighted fusion, the optimized main direction is obtained. :

[0031]

[0032] Adaptive weights and ( Based on the current point cloud density and the coefficient of determination of the main direction Dynamic adjustment. When the point cloud density is high and the main direction is clear, Increase; conversely, rely more on historical trajectories. Increase.

[0033] The principle for finding trackpoints in each branch direction is the same as that for finding trackpoints in the main direction. Taking the main direction as an example:

[0034] Define a vertical cross-sectional plane for the main direction, and calculate the candidate point of the cross-section center by fitting the geometry. Then, find the optimal target point or alternative point as the track point.

[0035] Specifically:

[0036] Step 401: Along the optimized main direction Distance from the previous valid center candidate point for ( , Define a vertical cross-sectional plane at the radius of perception. :

[0037]

[0038] Initial effective center candidate points Starting point .

[0039] Step 402: [The text appears to be incomplete and contains several grammatical errors. A more accurate translation would require Point clouds in the vicinity exceeding a set threshold thickness are projected onto this plane;

[0040] The RANSAC algorithm is used to fit a predefined geometry, with the geometric center as the reference point. As a candidate point for the center of the cross section :

[0041]

[0042] Step 403: Utilize confidence level The evaluation will identify candidate center points for cross-sections that exceed a set threshold. Considered a valid target point:

[0043]

[0044] The ratio of RANSAC in-points. This represents the angular coverage of the point cloud on the cross section.

[0045] Step 404: Determine the candidate center point of the cross section Is it available? If so, use the comprehensive utility function to select the optimal target point; otherwise, proceed to step 405.

[0046] The combined utility function of safety coverage gain and directional bias The exploration point is dynamically selected, and the calculation formula is as follows:

[0047]

[0048] in The safe radius for drones; and All are weighting coefficients; From point The expected volume of the unknown space that can be explored. It is path cost. It is a change in direction and angle. This shows the drone's current location in real time.

[0049] Step 405: The candidate point at the center of the cross section is blocked. Replacement points are generated from other parts of the cross section, and the lateral distance field (LDF) is introduced to assist in decision-making to select the optimal replacement point.

[0050] First, in the vertical cross-section plane Above, with geometric center Centered on, with intervals Sample on concentric circles to generate a set of candidate points. As an alternative point;

[0051] Then, calculate each candidate point Feasibility score (PS):

[0052]

[0053] in, It is the Euclidean sign distance field value of the candidate point; This is the lateral distance field value of the candidate point, reflecting the degree to which the point deviates from the pipe centerline; it is calculated as follows: ; This is the set of points on the pipe wall. It is the equivalent radius of the current vertical cross-section plane; These are the weighting coefficients.

[0054] Finally, the point with the highest feasibility score (PS) is selected as the optimal replacement point.

[0055] Step 406: For each branch direction, use the optimal target point or optimal alternative point as a waypoint and save the coordinates of each waypoint to the exploration queue. middle.

[0056] Step 5: Return to Step 2 and continue exploring the main direction of the point cloud data collected at the next time step t+1, and store the waypoints of each branch direction into the exploration queue. .

[0057] Step Six: Until the main direction of exploration becomes impassable at a certain point, remove the exploration queue. Select the highest priority branch trackpoint and explore along the corresponding branch until exploration is complete. Then remove the branch trackpoint from the exploration queue. delete;

[0058] "Impassable in the main direction" means that the entire vertical cross-sectional plane in the main direction is impassable, meaning that the ESDF value at all points on that cross-sectional plane is less than the safety threshold. Furthermore, the LDF file indicates that there is insufficient space.

[0059] Branch scheduling employs a dynamic priority strategy, with the priority function designed as follows:

[0060]

[0061] in, Each factor has its own weighting coefficient, which balances the weights of distance, scale, confidence, and global contribution. It is a branch point (i.e., exploration queue) The distance from the branch entry position stored in the database to the drone's current real-time position; It is a branch point The cross-sectional dimensions of the branch (i.e., the geometric size of the branch cross section, such as the equivalent radius or area); It is a branch point The confidence level of the branch it belongs to; It is a branch point The potential contribution of the branch to global coverage; These are normalization constants, used to scale terms of different dimensions to a comparable range.

[0062] Step 7: Repeat from the exploration queue Select the highest priority branch waypoint for exploration until the exploration queue is filled. All branch track points are deleted, and the system returns to the starting point to form a complete exploration path.

[0063] The advantages of this invention are:

[0064] 1. This invention provides a three-dimensional path planning method for autonomous exploration in enclosed spaces. It adopts a targeted branch management strategy to avoid the blindness of frontier-based methods in pipeline environments, resulting in a more systematic exploration path with low repetition rate and strong full coverage. It improves exploration efficiency and completeness in mining environments.

[0065] 2. The present invention provides a three-dimensional path planning method for autonomous exploration in enclosed spaces. The multi-sensor fusion scheme effectively overcomes the limitations of a single sensor in underground environments, resulting in more accurate pose estimation, more complete mapping, stronger robustness, and superior mapping quality.

[0066] 3. This invention provides a three-dimensional path planning method for autonomous exploration in enclosed spaces. It adaptively selects a path strategy based on the tunnel width. For narrow tunnels, B-spline curves are used for trajectory optimization, while for wide tunnels, a zigzag scanning mode is triggered. This ensures scanning coverage in both narrow and wide tunnel scenarios and eliminates blind spots. The resulting UAV path trajectory is smooth and controllable. The B-spline optimized trajectory conforms to the dynamic characteristics of the UAV, resulting in more stable flight, higher control precision, and lower energy consumption.

[0067] 4. This invention provides a method for autonomous three-dimensional path planning in enclosed spaces. It switches exploration modes; when the environment is a large, irregular, non-tubular space, it switches from a pipe-based exploration mode to a space-filling exploration mode based on the environmental structuring index, maximizing information gain using leading-edge points. It assesses exploration completeness by fusing multiple indicators such as regional growth rate, queue status, leading-edge point effectiveness, and topological connectivity to ensure comprehensive coverage of the entire space. The exploration process requires no human intervention, autonomously completing the entire process from perception and decision-making to planning. It boasts a high degree of intelligence and is suitable for harsh underground working environments. Attached Figure Description

[0068] Figure 1 This is a flowchart of a three-dimensional path planning method for autonomous exploration in a closed space according to the present invention;

[0069] Figure 2 This is a schematic diagram of a three-dimensional path planning method for autonomous exploration in a closed space according to the present invention.

[0070] Figure 3 This is a schematic diagram of the closed-space point cloud cross section and the center of the cross section of the present invention;

[0071] Figure 4 This is a schematic diagram illustrating the generation of an alternative point when the center point of the point cloud cross-section is impassable.

[0072] Figure 5 This is a schematic diagram of target point update at the branch of the present invention;

[0073] Figure 6 This is a schematic diagram of the wide tunnel exploration planning path of the present invention. Detailed Implementation

[0074] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some, not all, of the embodiments of the present invention. All other embodiments obtained by those skilled in the art based on the described embodiments of the present invention are within the scope of protection of the present invention.

[0075] This invention proposes a complete autonomous exploration and mapping solution for the structural characteristics of enclosed and unknown environments (such as tunnels, mines, and underground pipelines). The core innovation lies in prioritizing mapping completeness, using LiDAR to perceive environmental geometric features in real time, dynamically selecting the optimal exploration path, and ensuring comprehensive coverage of the entire space. Compared to traditional methods, this invention is particularly suitable for handling enclosed environments lacking GPS signals and with regular but complex structures.

[0076] like Figure 1 and Figure 2 As shown, the specific steps of the autonomous exploration three-dimensional path planning method in a closed space are as follows:

[0077] Step 1: In a closed pipeline environment without GPS, collect point cloud and inertial data in real time using a tightly coupled LiDAR and IMU scheme to compensate for motion distortion;

[0078] The drone, equipped with a lidar, can measure the three-dimensional coordinates of tunnel walls, obstacles, and other objects in a closed environment without GPS by emitting laser beams, generating point cloud data.

[0079] Step 2: For the point cloud data collected at time t, the Euclidean Symbolic Distance Field (ESDF) and octree map structure are combined to map the point cloud data onto three-dimensional cube cells to form a local environment voxel map, and a frontier point map layer is introduced to identify the exploration boundary.

[0080] In a closed environment without GPS, accurate environmental representation is the foundation of the algorithm. This system adopts a hybrid representation method that combines Euclidean Symbolic Distance Field (ESDF) with octree map, and introduces a frontier point map layer to form a triple environmental representation system that takes into account computational efficiency, map accuracy and coverage completeness.

[0081] Octree map structure: This structure uses a hierarchical tree structure to manage voxel data, reducing memory usage. The root node represents the entire environment space, and each non-leaf node is evenly divided into eight child nodes. Leaf nodes store specific voxel data. This structure supports fast neighbor lookup and range queries, which is crucial for real-time path planning.

[0082] ESDF map construction: Parallel maintenance of the Euclidean symbolic distance field, storing the distance to the nearest obstacle for each voxel:

[0083]

[0084] in For the set of elements representing obstacles, Voxel representation Negative values ​​can be found inside or outside obstacles. ESDF provides direct distance gradient information for trajectory optimization.

[0085] Specifically:

[0086] First, the point cloud data collected at time t is mapped to a three-dimensional cube element of the closed pipe environment, with each element being a voxel; the initial value of t is 1.

[0087] Then, the voxel data is managed using the hierarchical tree structure of the octree map, and the calculations are performed for each voxel. Occupancy probability This indicates the probability that the voxel is occupied by an obstacle.

[0088]

[0089] in This represents all observation point cloud data from time 1 to time t.

[0090] When the probability of a voxel being occupied by an obstacle exceeds a set threshold, the voxel is marked as an obstacle; otherwise, the distance from each voxel to the nearest obstacle is calculated using the Euclidean Symbolic Distance Field (ESDF) and stored to determine whether the drone can pass.

[0091] Frontier Point Map Layer:

[0092] 1. Definition of the leading edge: the state of the leading edge. Indicate whether the voxel is a leading edge point;

[0093] If and only if it has been explored And there are neighbors Satisfying unexplored ;

[0094] Among them, visibility status .

[0095] 2. Clustering: The DBSCAN algorithm is used to perform spatial clustering of the front points, generating a set of front clusters. Clustering distance threshold .

[0096] 3. Cluster quality score: It is used to evaluate the exploratory value of frontier clusters.

[0097] Step 3: On the local voxel map, perform principal component analysis using the current point cloud to explore the main direction of the UAV's flight and determine whether there are branches in the tunnel;

[0098] Specifically: First, for the current point cloud set Perform principal component analysis (PCA) and calculate the covariance matrix:

[0099]

[0100] in The center of mass of the point cloud.

[0101] Then, calculate the covariance matrix. Eigenvalues ​​and eigenvectors, largest eigenvalue Corresponding feature vector That is, the main direction of the current point cloud distribution. .

[0102] When the largest eigenvalue in PCA analysis with the second largest eigenvalue ratio When the value is close to 1 or less than the threshold (e.g., less than the threshold 2.0), it indicates that the tunnel has multiple extension directions, i.e., branches.

[0103] Step 4: Optimize the main direction. The UAV explores along the main direction and each branch direction separately to obtain the center candidate point in each direction, and further searches for waypoints, storing the waypoints of each branch direction into the exploration queue. .

[0104] like Figure 5 As shown, when there are multiple directional branches, after selecting the main direction, the waypoints of the other directions are added to the queue. middle.

[0105] Optimization refers to: optimizing the current estimated main direction. Compared with the historical average direction obtained based on recent trajectory smoothing By performing weighted fusion, the optimized main direction is obtained. :

[0106]

[0107] Adaptive weights and ( Based on the current point cloud density and the coefficient of determination of the main direction Dynamic adjustment. When the point cloud density is high and the main direction is clear, Increase; conversely, rely more on historical trajectories. Increase.

[0108] Prioritize along the main direction ( The corresponding direction is explored, while the other branch directions are encapsulated as structures containing information such as branch entry location, main direction, cross-sectional dimensions, and confidence level. The principle of exploring trackpoints in each branch direction is the same as that of exploring trackpoints in the main direction. Taking the main direction as an example:

[0109] A vertical cross-sectional plane is defined for the main direction of the tunnel. The Random Sample Consensus (RANSAC) algorithm is used to fit the geometry on the vertical cross-section to calculate candidate points for the cross-section center. After confidence evaluation, the optimal target point or alternative point is further searched as the track point. For example... Figure 3 The figure shown is a schematic diagram of the vertical cross-section of the obtained point cloud.

[0110] Specifically:

[0111] Step 401: Along the optimized main direction Distance from the previous valid center candidate point for ( , Define a vertical cross-sectional plane at the radius of perception. :

[0112]

[0113] Initial effective center candidate points Starting point .

[0114] Step 402: [The text appears to be incomplete and contains several grammatical errors. A more accurate translation would require Point clouds in the vicinity exceeding a set threshold thickness are projected onto this plane;

[0115] On a two-dimensional projection plane, the RANSAC algorithm is used to fit a predefined geometric shape, with the geometric center of the fitted shape as the reference point. Candidate for the center point of the cross section :

[0116]

[0117] Step 403: Utilize confidence level The evaluation will identify candidate center points for cross-sections that exceed a set threshold. Considered a valid target point;

[0118] Confidence level of this fit RANSAC interior point ratio Angular coverage of point cloud on cross section Joint decision: .

[0119] Only when Only when the value exceeds a set threshold (set to 0.6 in this embodiment) will the [process / process] be initiated. It is considered a valid target point.

[0120] Step 404: Determine the candidate center point of the cross section Is it available? If so, use the comprehensive utility function to select the optimal target point; otherwise, proceed to step 405.

[0121] The combined utility function of safety coverage gain and directional bias The exploration point is dynamically selected, and the calculation formula is as follows:

[0122]

[0123] in The safe radius for drones; and All are weighting coefficients; From point The expected volume of the unknown space that can be explored. It is path cost. It is a change in direction and angle. This shows the drone's current location in real time.

[0124] Step 405: When the candidate point at the center of the cross section is blocked, but the evaluation finds that other parts of the pipe cross section are passable, an alternative point is generated from other parts of the cross section, and the lateral distance field (LDF) is introduced to assist in decision-making and select the optimal alternative point.

[0125] For example, there is debris buildup on one side of the pipe but the other side is clear; the assessment refers to: the ESDF value is greater than the safety threshold. The voxels.

[0126] like Figure 4 The diagram shown illustrates the generation of alternative points when the candidate point at the center of the point cloud cross-section is impassable. The specific process is as follows:

[0127] First, we introduce the Lateral Distance Field (LDF) to assist in decision-making:

[0128]

[0129] in This is the set of points on the pipe wall.

[0130] In the vertical cross-section plane Above, with geometric center Centered on, with intervals Sample on concentric circles to generate a set of candidate points. As an alternative point;

[0131] Then, calculate each candidate point The passability score (PS) combines safety and practicability, and its calculation formula is as follows:

[0132]

[0133] in, It is the Euclidean sign distance field value of the candidate point, reflecting the safety; It is the lateral distance field value of the candidate point, reflecting the degree to which the point deviates from the centerline of the pipeline; It is the equivalent radius of the current vertical cross-section plane; These are the weighting coefficients; It is a weighting coefficient used to balance security and progress.

[0134] Finally, the point with the highest feasibility score (PS) is selected as the optimal replacement point.

[0135] Step 406: For each branch direction, use the optimal target point or optimal alternative point as a waypoint and save the coordinates of each waypoint to the exploration queue. middle.

[0136] Step 5: Return to Step 2 and continue exploring the main direction of the point cloud data collected at the next time step t+1, and store the waypoints of each branch direction into the exploration queue. .

[0137] Step Six: Until the main direction of the explored pipeline becomes impassable at a certain point, remove the exploration queue. Select the highest priority branch trackpoint and explore along the corresponding branch until exploration is complete. Then remove the branch trackpoint from the exploration queue. delete;

[0138] "Impassable in the main direction" means that the entire vertical cross-sectional plane in the main direction is impassable, meaning that the ESDF value at all points on that cross-sectional plane is less than the safety threshold. Furthermore, the LDF display shows insufficient space (e.g., minimum width < drone width + 2 * ...). For example, the pipe may be completely blocked or narrowed to the point that a drone, plus a safety margin, cannot pass through.

[0139] At this point, global backtracking and mode switching are employed:

[0140] Trigger global backtracking: Immediately pause exploration of the current pipeline branch. The drone will retreat along the path it came from, backtracking to the last most recent branch point (the branch point information is stored in the exploration queue). middle).

[0141] Queue Update: Mark the current pipe inlet as "explored" and update it from the exploration queue. Remove it from the queue. Then, the system re-evaluates the priority of other branch points in the queue based on information gain, and schedules the exploration order through a dynamic priority strategy, selecting a new target point to continue exploration.

[0142] Branch scheduling employs a dynamic priority strategy, with the priority function designed as follows:

[0143]

[0144] in, Each factor has its own weighting coefficient, which balances the weights of distance, scale, confidence, and global contribution. It is a branch point (i.e., exploration queue) The distance from the branch entry position stored in the database to the drone's current real-time position; It is a branch point The cross-sectional dimensions of the branch (i.e., the geometric size of the branch cross section, such as the equivalent radius or area); It is a branch point The confidence level of the branch it belongs to; It is a branch point The potential contribution of the branch to global coverage; These are normalization constants, used to scale terms of different dimensions to a comparable range.

[0145] This strategy can systematically guide UAVs to prioritize exploring nearby, large-scale, high-confidence branches that contribute significantly to global coverage, thereby improving global exploration efficiency.

[0146] Step 7: Repeat from the exploration queue Select the highest priority branch waypoint for exploration until the exploration queue is filled. All branch track points are deleted, and the system returns to the starting point to form a complete exploration path.

[0147] Example:

[0148] The hardware components of the device described in this invention are as follows:

[0149] Platform: Quadrotor drone, payload ≥ 5kg, flight time ≥ 10 minutes.

[0150] LiDAR: The LiDAR is installed with a 10° downward tilt to optimize perception of both near-ground and forward areas. It features a 32-line LiDAR with a 360° horizontal field of view, a ±15° vertical field of view, a ranging range of 0.5-100m, and a point cloud density of ≥200 points / ㎡ at a distance of 10m, ensuring scanning accuracy at medium to long ranges.

[0151] Computing unit: Industrial computer, supporting real-time point cloud processing (frame rate ≥ 10Hz) and path planning (computation time ≤ 1 second).

[0152] Auxiliary equipment: IMU (heading accuracy ≤0.1° / h) for positioning correction, SLAM module (indoor) for global positioning, and communication module (Wi-Fi / 4G) for data transmission and remote monitoring.

[0153] Effective detection range of lidar This is a key parameter, determined by both sensor characteristics and mapping resolution requirements. Assume the point cloud resolution requirement is at least [amount missing] per cubic meter. The LiDAR angular resolution is [number] points. Then the maximum effective distance It can be estimated as follows:

[0154]

[0155] In practice, The value was determined through actual measurements. A spherical coverage model was adopted, assuming that the UAV flies through the path, with... The area traversed by the sphere with radius [radius value] has been completely covered.

[0156] Sensor configuration and blind spot handling:

[0157] Installation optimization: The lidar is installed at a downward tilt of 10° to 15° to optimize perception of both near-ground and forward areas.

[0158] Active scanning mode: When the system initializes or enters a large unknown space, if a large-scale top / bottom blind spot is detected (determined by analyzing the distribution of point cloud normal vectors), the active scanning mode can be triggered. This mode controls the UAV to swing its pitch axis (e.g., ±10°) to dynamically acquire complete information about the environment above and below, compensating for the perception blind spots of static installation (the same applies to lateral space).

[0159] Inertial navigation compensation:

[0160] In GPS-denied environments, a tightly coupled LiDAR-IMU scheme is employed; IMU data undergoes pre-integration processing to compensate for LiDAR motion distortion.

[0161] IMU pre-integration: The measurements taken by the IMU between two camera / LiDAR keyframes are pre-integrated to obtain the relative pose change, avoiding repeated integration during the optimization process.

[0162]

[0163] in The original measurement value of the accelerometer. To achieve zero bias in the accelerometer, For measuring noise.

[0164] The core of this embodiment lies in directly utilizing point cloud geometric features to guide exploration, rather than traditional boundary-based methods. This method is particularly suitable for pipe-like enclosed environments, achieving efficient navigation by extracting the geometric features of tunnel cross-sections.

[0165] Outlier removal: The Random Sample Consensus (RANSAC) algorithm is used to fit the tunnel axis, iteratively estimating the optimal model parameters to resist noise and outlier interference. RANSAC fits a linear model by randomly sampling the minimum set of points, evaluates the number of points within the model, and selects the optimal fit.

[0166] For different tunnel widths, an adaptive path strategy is adopted to ensure full coverage. This embodiment combines the advantages of global path planning and local trajectory optimization, ensuring both coverage integrity and smooth, feasible trajectories.

[0167] Narrow tunnel route planning (width) ):

[0168] B-spline trajectory parameterization: The trajectory is represented by a uniform B-spline curve. A p-order B-spline is formed by control points. And the definition of node vectors. The expression for a third-order B-spline curve is:

[0169]

[0170] in It is a third-order B-spline basis matrix. These are the normalized parameters.

[0171] Trajectory optimization model: control points Solve the following optimization problem:

[0172]

[0173] The cost functions are defined as follows:

[0174] Smoothing Cost The jerk (acceleration) of the penalty trajectory ensures ride comfort and controllability.

[0175]

[0176] Collision Cost Using the ESDF map, the penalty trajectory is determined by its proximity to obstacles.

[0177]

[0178] in, It is a parameter for adjusting the sensitivity of the safe distance.

[0179] Dynamic cost Ensure the trajectory meets the drone's maximum speed. and acceleration constraint.

[0180]

[0181] Unknown space penalty item To prevent the trajectory from entering unexplored areas and enhance safety.

[0182]

[0183] in, It is an indicator function, when the trajectory point The value is 1 when it is located in a voxel with the status of "unexplored", and 0 otherwise.

[0184] Coverage of reward items (New): The incentive trajectory is closer to unexplored areas, enhancing the completeness of coverage.

[0185]

[0186] in It is an estimation of the unknown spatial volume near the trajectory point. It is a set of frontier points.

[0187] Wide tunnel coverage path (width) ):

[0188] like Figure 6 As shown, when the tunnel width exceeds the sensor's single-pass coverage capability, a zigzag scanning mode is triggered:

[0189] Path generation formula:

[0190]

[0191] in, The swing amplitude ensures that the sensing sphere can cover both sides of the tunnel; The period length of the zigzag path can be adaptively adjusted according to the point cloud density; It is the normal vector perpendicular to the principal direction in the plane of the cross section.

[0192] Path smoothing and optimization:

[0193] Will As the target for fitting the B-spline curve, a smooth trajectory that conforms to the dynamic constraints of the UAV is generated. The trajectory is then time-parameterized to meet the maximum velocity and acceleration limits.

[0194] Coverage guarantee mechanism: Zigzag path ensures coverage. A sphere with radius [radius value] can continuously cover the entire tunnel cross-section. Path spacing. satisfy To avoid coverage blind spots.

[0195] Adaptive adjustment mechanism:

[0196] Swing amplitude adaptive: The oscillation amplitude is adjusted based on the density of the leading edge points. Period length learning: The period length is dynamically adjusted based on historical exploration efficiency. This improves coverage efficiency.

[0197] In spaces lacking a clear dominant geometry, such as large underground caverns and abandoned factories, exploration strategies based on the center point of a cross section will fail. Existing UAV autonomous exploration algorithms are more suitable for such scenarios, and the algorithms assist in mode switching by performing environmental recognition.

[0198] The algorithm calculates two key metrics in real time:

[0199] Main direction uncertainty measurement Defined as the largest eigenvalue in PCA analysis. with the second largest eigenvalue The ratio, . The larger the value, the more tubular the environment; the closer it is to 1, the more isotropic the environment.

[0200] Cross-section fitting confidence : That is, confidence level.

[0201] Environmental structuring index: Defines a continuous environmental structuring index. :

[0202]

[0203] in For the Sigmoid function, It is a measure of the regularity of cross-sectional shape.

[0204] When both conditions are met and When the current environment is determined to be a large, non-tubular, irregular space, a switch from "pipeline exploration mode" to "space filling exploration mode" is triggered.

[0205] Frontier extraction: Frontiers are the boundaries between explored and unknown regions. Mathematically, the set of frontiers... Defined as:

[0206] Explored but not explored

[0207]

[0208] in, It is a voxel The neighborhood of.

[0209] Target point selection: Selecting candidate points that maximize information gain. Information gain. It is usually defined as starting from the current position. Reaching the candidate frontier Subsequently, the expected volume of the unknown space to be explored. Meanwhile, path cost is also considered. :

[0210]

[0211] Hybrid Exploration Strategy (New): Employs hybrid objective point generation in transitional areas.

[0212]

[0213] in The center point of the cross section, It is the optimal frontier point.

[0214] The system is recalculated periodically (e.g., every 10 cubic meters explored). and When the environment restores its regular structure ( and When this happens, the algorithm automatically switches back to the efficient "pipeline exploration mode".

[0215] Regional growth rate threshold: set For a moment Volume explored, when continuous When the growth rate is lower than the threshold within a certain time period:

[0216]

[0217] in It is usually set at 0.1%-0.5%, and can be adjusted according to the task requirements.

[0218] Regional growth disappearance judgment: When no region exceeding the minimum threshold is detected in 10 consecutive seconds of scanning, the exploration is considered complete.

[0219] Queue status check: Requires exploration of the queue. If empty, ensure that all branches are processed.

[0220] Frontier validity verification:

[0221]

[0222] in The number of clusters at the frontier points, This represents the cluster quality score.

[0223] Incremental Connectivity Analysis: Connectivity is locally updated only when the map is updated. A Union-Find algorithm is used to identify connected regions in the map. This ensures that all connected components have been visited and that there are no isolated, unexplored regions.

[0224] Incremental update steps:

[0225] Initialization: Each newly explored free voxel is initialized as an independent set.

[0226] Neighbor check: When a new voxel is added, check the existing voxels in its 6-neighborhood (up, down, left, right, front, back).

[0227] Merge operation: If the neighbors are in free space, use the union operation of the disjoint-set data structure to merge the new voxel with its neighbors into the same set.

[0228] Query: Check and retrieve the number of sets in the set at any time. If only one set remains, it means all free spaces are connected.

[0229] Multi-evidence fusion decision (new): The following evidence is fused using the DS evidence theory:

[0230] Volume growth rate consistently below the threshold

[0231] The number of explicit front points is reduced to zero.

[0232] The number of free-space connected components in the map is 1.

[0233] The utility of the optimal frontier point remains below the threshold for a period of time.

[0234] The exploration is considered complete only when all the evidence supports the "complete" hypothesis with high confidence.

[0235] A systematic exploration strategy for multi-branch pipelines: a state machine management method based on branch queues to ensure full coverage exploration of complex pipeline networks.

[0236] Target point generation method based on cross-section center point: The tunnel geometric center is robustly extracted from the real-time point cloud using algorithms such as PCA, and used as the basis for waypoint generation.

[0237] Adaptive wide tunnel coverage algorithm: Based on the real-time perceived tunnel width, it dynamically switches between straight-line flight and "zigzag" scanning modes, and uses B-spline curves for trajectory optimization.

[0238] A quantitative judgment mechanism for exploration completion: combining the dual judgment criteria of branch queue status and unexplored area growth rate.

[0239] The core of this invention adopts a pure solid-state lidar solution; alternative solutions may include:

[0240] Depth camera: In scenes rich in visual information, the use of a depth camera for visual SLAM is essentially the same as the use of LiDAR for LiDAR-based SLAM in this invention.

[0241] Lidar combined with depth camera solution: In some scenarios, solid-state LiDAR and vision can be used together as SLAM sensors to achieve better localization and mapping.

[0242] Alternatives to path planning algorithms: Artificial potential field method and similar algorithms can replace B-splines: When computational resources are extremely limited, the artificial potential field method, which requires less computation, can be used for local trajectory optimization.

[0243] For the algorithm to reach the next target point: In calculating the path to the next target point, many other types of path planning algorithms can be used to achieve the same goal.

[0244] Multi-unmanned equipment collaborative alternative: Utilizing multiple unmanned equipment to collaboratively explore and map will significantly shorten the exploration time.

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

Claims

1. A method for autonomous exploration of three-dimensional paths in a closed space, characterized in that, The specific steps are as follows: Step 1: In a closed pipeline environment without GPS, collect point cloud and inertial data in real time using a tightly coupled LiDAR and IMU solution; Step 2: For the point cloud data collected at time t, the Euclidean Symbolic Distance Field (ESDF) and octree map structure are combined to map the point cloud data onto three-dimensional cube cells to form a local voxel map. Step 3: On the local voxel map, perform principal component analysis using the current point cloud to explore the main direction of the UAV's flight and determine whether there are branches. Step 4: Optimize the main direction. The UAV explores along the main direction and each branch direction separately to obtain the center candidate point in each direction, and further searches for the waypoints in each branch direction and stores them in the exploration queue. ; The principle for finding trackpoints in each branch direction is the same as that for finding trackpoints in the main direction. Taking the main direction as an example: Define a vertical cross-section plane for the main direction, and calculate the candidate point of the cross-section center by fitting the geometry, and further find the optimal target point or alternative point as the track point; Step 5: Return to Step 2 and continue exploring the main direction of the point cloud data collected at the next time step t+1, and store the waypoints of each branch direction into the exploration queue. ; Step Six: Until the main direction of exploration becomes impassable at a certain point, remove the exploration queue. Select the highest priority branch trackpoint and explore along the corresponding branch until exploration is complete. Then remove the branch trackpoint from the exploration queue. delete; Branch scheduling employs a dynamic priority strategy, with the priority function designed as follows: in, Each factor has its own weighting coefficient, which balances the weights of distance, scale, confidence, and global contribution. It is an exploration queue The branch point at the branch entry position stored in the middle The distance to the drone's current location in real time; It is a branch point The cross-sectional dimensions of the branch in which it is located; It is a branch point The confidence level of the branch it belongs to; It is a branch point The potential contribution of the branch to global coverage; These are normalization constants, used to scale terms of different dimensions to a comparable range; Step 7: Repeat from the exploration queue Select the highest priority branch waypoint for exploration until the exploration queue is filled. All branch track points are deleted, and the system returns to the starting point to form a complete exploration path.

2. The method as described in claim 1, characterized in that, In step one, the UAV, equipped with a lidar, in a closed environment without GPS, emits a laser beam to measure the three-dimensional coordinates of the tunnel walls and obstacles, generating point cloud data.

3. The method as described in claim 1, characterized in that, Step two specifically involves: First, the point cloud data collected at time t is mapped to three-dimensional cubic elements of the closed pipe environment, with each element being a voxel; the initial value of t is 1. Then, the voxel data is managed using the hierarchical tree structure of the octree map, and the calculations are performed for each voxel. Occupancy probability This indicates the probability that the voxel is occupied by an obstacle. in This represents all observed point cloud data from time 1 to time t; When the probability of a voxel being occupied by an obstacle exceeds a set threshold, the voxel is marked as an obstacle; otherwise, the distance from each voxel to the nearest obstacle is calculated using the Euclidean Symbolic Distance Field (ESDF) and stored to determine whether the drone can pass. in For the set of elements representing obstacles, Voxel representation Inside or outside the obstacle.

4. The method as described in claim 1, characterized in that, Step three specifically involves: First, processing the current point cloud set... Perform principal component analysis and calculate the covariance matrix: in The centroid of the point cloud; Then, calculate the covariance matrix. eigenvalues, largest eigenvalue Corresponding feature vector That is, the main direction of the current point cloud distribution. ; When the largest eigenvalue with the second largest eigenvalue ratio When the value is close to 1 or less than the threshold, it indicates that the tunnel has multiple extension directions, i.e., branches.

5. The method as described in claim 4, characterized in that, In step four, optimization refers to: optimizing the currently estimated principal direction. Compared with the historical average direction obtained based on recent trajectory smoothing By performing weighted fusion, the optimized main direction is obtained. : Adaptive weights and , Based on the current point cloud density and the coefficient of determination of the main direction Dynamic adjustment; when cloud density is high and the main direction is clear, Increase; conversely, rely more on historical trajectories. Increase.

6. The method as described in claim 5, characterized in that, Step four specifically involves: Step 401: Along the optimized main direction Distance from the previous valid center candidate point for Define the vertical cross-section plane at that location. : ； Initial effective center candidate points Starting point , For sensing radius; Step 402, the cross-sectional plane Point clouds in the vicinity exceeding a set threshold thickness are projected onto this plane; The RANSAC algorithm is used to fit a predefined geometry, with the geometric center as the reference point. Candidate for the center point of the cross section : ； Step 403, using confidence level The evaluation will identify candidate center points for cross-sections that exceed a set threshold. Considered a valid target point: ； The ratio of RANSAC in-points. This represents the angular coverage of the point cloud on the cross section. Step 404: Determine the candidate center point of the cross section Is it available? If so, use the comprehensive utility function to select the optimal target point. Otherwise, proceed to step 405; The combined utility function of safety coverage gain and directional bias The exploration point is dynamically selected, and the calculation formula is as follows: ； in The safe radius for drones; and All are weighting coefficients; From point The expected volume of the unknown space that can be explored. It is path cost. It is a change in direction and angle. This shows the drone's current location in real time. Step 405: The candidate point at the center of the cross section is blocked. Replacement points are generated from other parts of the cross section, and the lateral distance field LDF is introduced to assist decision-making to select the optimal replacement point. First, in the vertical cross-section plane Above, with geometric center Centered on, with intervals Sample on concentric circles to generate a set of candidate points. As an alternative point; Then, calculate each candidate point Feasibility score PS: ； in, It is the Euclidean sign distance field value of the candidate point; This is the lateral distance field value of the candidate point, reflecting the degree to which the point deviates from the pipe centerline; it is calculated as follows: ; For the set of points on the pipe wall; It is the equivalent radius of the current vertical cross-section plane; These are the weighting coefficients; Finally, the point with the highest feasibility score (PS) is selected as the optimal replacement point. Step 406: For each branch direction, use the optimal target point or optimal alternative point as a waypoint and save the coordinates of each waypoint to the exploration queue. middle.

7. The method as described in claim 1, characterized in that, In step six, "impassable in the main direction" means that the entire vertical cross-sectional plane in the main direction is impassable, that is, the ESDF value of all points on the cross-sectional plane is less than the safety threshold. Furthermore, the LDF file shows that there is not enough space.

Images