A local three-dimensional point cloud artificial potential field tunnel autonomous flight planning method

Abstract

The application discloses a kind of local three-dimensional point cloud artificial potential field tunnel autonomous flight planning methods, it is related to unmanned aerial vehicle autonomous flight control technical field, comprising the following steps: S1, data acquisition and pre-processing;S2, prior target generation;S3, artificial potential field correction;S4, post target release.The application, through local point cloud filtering, cutting and other pre-processing operations, sparse data is revised, and gradient iteration is completed with artificial potential field, can quickly respond to tunnel environmental changes, and point cloud preprocessing can eliminate interference outliers, artificial potential field revision can push target point away from obstacles, combined with target point offset limit and prior target distance constraint, ensure that flight direction is stable, effectively avoid collision risk, without relying on prior map and GPS signal, do not require good light conditions, can be adapted to tunnel, pipe gallery, mine and other various irregular tubular dangerous environments, solve the problem of insufficient generality of prior art.

Description

Technical Field

[0001] This invention relates to the field of autonomous flight control technology for unmanned aerial vehicles (UAVs), specifically to an autonomous flight planning method for a local three-dimensional point cloud artificial potential field tunnel. Background Technology

[0002] With the development and improvement of drone technology, its practical application scope is becoming wider and wider, and the mission requirements are becoming more complex and diversified. The use scenarios for drones are no longer limited to outdoor scenes, and their uses are no longer limited to carrying out agricultural and photography activities in outdoor environments. Considering the exploration and inspection of various tunnels, caves and other dangerous environments, drones have more advantages than humans in such environments. Therefore, it is necessary to carry out research on how drones can achieve autonomous flight in such environments. However, when quadcopter drones fly in tunnel environments, there are usually many unpredictable obstacles and narrow spaces, making drones prone to collision accidents. Secondly, the direction and terrain of tunnels are varied, there are no reference points inside and no GPS signals, making it difficult for drones to obtain reference routes. Currently, solutions to the above problems mainly include visual line-following methods, millimeter-wave and ultrasonic ranging module array methods, and more complex SLAM mapping and planning methods. Visual line-following methods use obvious features such as railways as guides for autonomous flight, requiring good lighting conditions, and have poor versatility and stability, unable to avoid obstacles and thus failing to meet exploration needs. Ranging module array methods are typically used for exploration flights in narrow, regularly shaped spaces such as drainage pipes and ventilation ducts; they are generally low-cost, logically simple, and highly stable, but still cannot achieve safe flight in unknown, irregular tunnels. SLAM mapping and planning methods are used in various indoor environments. Commonly used methods in flight missions involve generating safe flight trajectories by referencing 3D spatial data from binocular vision or lidar, combined with optimization theory. Their advantage lies in their versatility, enabling them to complete various flight trajectory planning tasks. However, they have significant drawbacks. First, because they use gradient descent optimizers and constraints to generate flight trajectories, the trajectory exhibits high uncertainty and is prone to getting trapped in local optima, resulting in poor safety. Second, the generation process typically consumes substantial computing power, with significant redundant calculations in tunnel environments. For lightweight unmanned aerial vehicle (UAV) systems, this leads to poor real-time performance, an inability to respond quickly to environmental changes, and even complete inability to execute due to performance limitations. Summary of the Invention

[0003] To address the shortcomings of existing technologies, this invention provides an autonomous flight planning method for tunnels in local three-dimensional point cloud artificial potential fields, thus solving the problems mentioned in the background section.

[0004] To achieve the above objectives, the present invention provides the following technical solution: an autonomous flight planning method for a local three-dimensional point cloud artificial potential field tunnel, comprising the following steps: S1. Data Acquisition and Preprocessing: By subscribing to the raw point cloud data of the drone's LiDAR and the drone's current pose information in real time through the ROS interface, the point cloud data is then preprocessed to remove outliers caused by interference with the LiDAR, while simultaneously sparsening the originally high-density point cloud to reduce computational pressure. S2. Prior target generation: The maximum distance limit between the prior target and the drone is set to default to Specifically, adjustments can be made based on the actual situation to determine the centroid of the point cloud in front; S3, Artificial Potential Field Correction: Extract the radius near the prior target point The neighborhood point set is obtained and an artificial potential field is corrected. This search is completed using the kdtree module in the pcl library. S4. Post-hoc target release: Let the current position of the drone be... Then, the desired nose direction of the UAV, i.e., the x-axis vector of the UAV body in the inertial coordinate system, is set as follows: , Let the z-axis vector of the UAV body in the vertical direction, i.e., in the inertial coordinate system, be... ,in Used to convert row vectors Convert to column vector ,but: , in, Let y be the vector of the UAV body in the inertial coordinate system. The desired attitude of the drone is converted into a quaternion q, combined with the posterior target position, and published in PoseStamped format for execution by the underlying system.

[0005] Furthermore, in step S1, the point cloud data preprocessing includes point cloud filtering and cropping, specifically as follows: Let the input point cloud be ,in, These are the three-dimensional coordinates of each point in the point cloud. The number of points in the point cloud is used to perform voxel filtering on the point cloud, and the voxel size is... Adjust according to the actual situation. ,in, This is the function for the voxel filtering algorithm. To input point clouds, This is the set of point clouds after voxel filtering. Voxel filtering is the process of filtering a point cloud with dimensions of 1000 x ... If a point exists in a voxel, the center of that voxel is retained as the replacement point. Then, statistical filtering is performed to remove outliers, using the following formula: , Let the average number of neighborhoods be... That is, for each point, consider its 100 neighbors, and the standard deviation of the distances from these neighbors is... The mean distance between all points in the point cloud and their neighboring points is 1. If the average distance between this point and its neighbors is greater than If so, then discard it, and It is a point The average distance between its 100 neighboring sites, let point The neighborhood is ,but: , in, For point To adjacent points The distance between them; After the above processing, only the forward distance of the UAV body coordinate system is retained. Points: , in, , This is the set of valid points output after filtering.

[0006] Furthermore, in step S2, the formula for calculating the centroid of the foreground point cloud is as follows: , in, For the number of points in the point cloud, Yes The average value of the points in the set, if The length is greater than Then it is necessary to Proportional shortening to Length, otherwise take the value directly. This is the prior target point.

[0007] Furthermore, the aforementioned The formula for proportional shortening is as follows: , in, The calculation results are for the prior target points. This is the distance scaling factor.

[0008] Furthermore, in step S3, the artificial potential field correction process is as follows: , in, , For point clouds Mid-range No more than At that point, the artificial potential function is as follows: , Then update the target point using the gradient ascent method: , in, It is an intermediate target point in the iterative process; For the potential field function in Gradient vector at point; learning rate and maximum number of iterations Set according to the actual situation; and set constraints to limit the maximum offset of the target point, i.e. Finally, the corrected target point is obtained. ,in This is the final posterior target point 3D coordinate vector output after correction by the artificial potential field. For the first The target point's three-dimensional coordinate vector for the next iteration.

[0009] Furthermore, step S3 also includes a local slope adaptive adjustment process, the specific process of which is as follows: Posterior target point corrected by artificial potential field Extract the radius centered on the radius. Point cloud set within the range , ,in, for A single three-dimensional coordinate point in the data. Extracting the radius from the point cloud for slope analysis; Using the least squares method By performing plane fitting on the point cloud, the ground plane equation is obtained: , in, , , These are the coordinates of a point in three-dimensional space. , , normal vector of the plane The components of the plane determine its direction, where the normal vector is perpendicular to the plane; These are parameters related to the distance between the plane and the origin, used to adjust the plane's position in space; The fitting objective is to minimize the error function: , in,( , , ) is a point The three-dimensional coordinates are subject to the following constraints: ; The normal vector of the ground plane is The vertical direction vector in the inertial coordinate system, i.e., the z-axis vector of the UAV body in the inertial coordinate system, is: Then the slope angle Normal vector with vertical direction vector The included angle is calculated using the following formula: .

[0010] Furthermore, a preset slope angle safety threshold is established. ,like Then keep And the pitch angle constraint of the UAV remains unchanged; if Then, the vertical coordinates of the posterior target point are adjusted using the following formula: , in, Before adjustment The vertical coordinates; The adjusted vertical coordinates; This is the vertical adjustment coefficient; Then, the pitch angle constraint is adjusted, and the upper limit of the basic pitch angle constraint is set to... Adjust the upper limit of the pitch angle constraint ,in This is the pitch angle scaling factor, and , This is the maximum pitch angle allowed by the mechanical structure of the drone.

[0011] Furthermore, the adjusted posterior target point As input for subsequent posterior target releases, at the same time The data is transmitted to the underlying flight controller to constrain the drone's pitch angle range.

[0012] Furthermore, the posterior target point When used as input for subsequent posterior target releases, the steps in S4 will be... Replace with .

[0013] This invention provides an autonomous flight planning method for tunnels in local three-dimensional point cloud artificial potential fields, which has the following beneficial effects: 1. This local 3D point cloud artificial potential field tunnel autonomous flight planning method uses local point cloud preprocessing operations such as filtering and pruning to sparse the data, and then performs gradient iteration to complete artificial potential field correction. This avoids redundant calculations, reduces computing power consumption, and is suitable for lightweight UAV onboard systems. It can quickly respond to changes in the tunnel environment. Point cloud preprocessing can remove interfering outliers, and artificial potential field correction can push the target point away from obstacles. Combined with target point offset constraints and prior target distance constraints, it ensures stable flight direction and effectively avoids collision risks. Moreover, it has a wide range of applications, does not rely on prior maps and GPS signals, and does not require good lighting conditions. Through local point cloud analysis and artificial potential field optimization, it can be adapted to various irregular tubular and dangerous environments such as tunnels, pipe corridors, and mines, solving the problem of insufficient versatility of existing technologies.

[0014] 2. This local 3D point cloud artificial potential field tunnel autonomous flight planning method calculates the slope angle by fitting the ground plane with local point cloud data, dynamically adjusts the vertical coordinates of the posterior target point and the UAV pitch angle constraint, accurately adapts to tunnel slope changes, avoids fuselage instability or abnormal attitude caused by sudden terrain changes, and enhances the adaptability of UAVs in complex terrain. It effectively covers special terrains such as tunnel uphill, downhill and inclined sections. By predicting the slope and making targeted attitude adjustments, it avoids the collision risk caused by slope changes in advance, reduces the flight hazards of UAVs in undulating terrain areas, and improves the reliability of operations in difficult environments. Attached Figure Description

[0015] Figure 1 This is an overall flowchart of the autonomous flight planning method for a local three-dimensional point cloud artificial potential field tunnel according to the present invention; Figure 2 This is a schematic diagram of the artificial potential field correction target point adjustment and heading for the autonomous flight planning method of a local three-dimensional point cloud artificial potential field tunnel according to the present invention. Detailed Implementation

[0016] The embodiments of the present invention will be described in further detail below with reference to the accompanying drawings and examples. The following examples are for illustrative purposes only and should not be construed as limiting the scope of the invention.

[0017] like Figures 1-2 As shown, the present invention provides a technical solution: an autonomous flight planning method for a local three-dimensional point cloud artificial potential field tunnel, comprising the following steps: S1. Data Acquisition and Preprocessing: By subscribing to the raw point cloud data of the drone's LiDAR and the drone's current pose information in real time through the ROS interface, the point cloud data is then preprocessed to remove outliers caused by interference with the LiDAR, while simultaneously sparsening the originally high-density point cloud to reduce computational pressure. Point cloud data preprocessing includes point cloud filtering and cropping, with the specific inputs as follows: Let the input point cloud be ,in, These are the three-dimensional coordinates of each point in the point cloud. The number of points in the point cloud is used to perform voxel filtering on the point cloud, and the voxel size is... Adjust according to the actual situation. ,in, This is the function for the voxel filtering algorithm. To input point clouds, This is the set of point clouds after voxel filtering. Voxel filtering is the process of filtering a point cloud with dimensions of 1000 x ... If a point exists in a voxel, the center of that voxel is retained as the replacement point. Then, statistical filtering is performed to remove outliers, using the following formula: , Let the average number of neighborhoods be... That is, for each point, consider its 100 neighbors, and the standard deviation of the distances from these neighbors is... The mean distance between all points in the point cloud and their neighboring points is 1. If the average distance between this point and its neighbors is greater than If so, then discard it, and It is a point The average distance between its 100 neighboring sites, let point The neighborhood is ,but: , in, For point To adjacent points The distance between them; After the above processing, only the forward distance of the UAV body coordinate system is retained. Points: , in, , This is the set of valid points output after filtering. S2. Prior target generation: The maximum distance limit between the prior target and the drone is set to default to Specifically, adjustments can be made based on the actual situation to determine the centroid of the point cloud in front; The formula for finding the centroid of a point cloud in front is as follows: , in, For the number of points in the point cloud, Yes The average value of the points in the set, if The length is greater than Then it is necessary to Proportional shortening to Length, otherwise take the value directly. The prior target point; The formula for proportional shortening is as follows: , in, The calculation results are for the prior target points. This is the distance scaling factor; S3, Artificial Potential Field Correction: Extract the radius near the prior target point The neighborhood point set is obtained and an artificial potential field is corrected. This search is completed using the kdtree module in the pcl library. The specific process for correcting the artificial potential field is as follows: , in, , For point clouds Mid-range No more than At that point, the artificial potential function is as follows: , Then update the target point using the gradient ascent method: , in, It is an intermediate target point in the iterative process; For the potential field function in Gradient vector at point; learning rate and maximum number of iterations Set according to the actual situation; and set constraints to limit the maximum offset of the target point, i.e. Finally, the corrected target point is obtained. ,in This is the final posterior target point 3D coordinate vector output after correction by the artificial potential field. For the first The three-dimensional coordinate vector of the target point in the next iteration; S4. Post-hoc target release: Let the current position of the drone be... Then, the desired nose direction of the UAV, i.e., the x-axis vector of the UAV body in the inertial coordinate system, is set as follows: , Let the z-axis vector of the UAV body in the vertical direction, i.e., in the inertial coordinate system, be... ,in Used to convert row vectors Convert to column vector ,but: , in, Let y be the vector of the UAV body in the inertial coordinate system. The desired attitude of the drone is converted into a quaternion q, combined with the posterior target position, and published in PoseStamped format for execution by the underlying system. Based on the above description, this invention sparsifies data through preprocessing operations such as local point cloud filtering and cropping, and completes artificial potential field correction in conjunction with gradient iteration. This avoids redundant calculations, reduces computing power consumption, and is suitable for lightweight UAV airborne systems. It can quickly respond to changes in the tunnel environment. Furthermore, point cloud preprocessing can remove interfering outliers, and artificial potential field correction can push the target point away from obstacles. Combined with target point offset limits and prior target distance constraints, it ensures stable flight direction and effectively avoids collision risks. Moreover, it has a wide range of applications, does not rely on prior maps and GPS signals, and does not require good lighting conditions. Through local point cloud analysis and artificial potential field optimization, it can be adapted to various irregular tubular and dangerous environments such as tunnels, pipe corridors, and mines, solving the problem of insufficient versatility of existing technologies. Step S3 also includes a local slope adaptive adjustment process, the specific process of which is as follows: Posterior target point corrected by artificial potential field Extract the radius centered on the radius. Point cloud set within the range , ,in, for A single three-dimensional coordinate point in the data. Extracting the radius from the point cloud for slope analysis; Using the least squares method By performing plane fitting on the point cloud, the ground plane equation is obtained: , in, , , These are the coordinates of a point in three-dimensional space. , , normal vector of the plane The components of the plane determine its direction, where the normal vector is perpendicular to the plane; These are parameters related to the distance between the plane and the origin, used to adjust the plane's position in space; The fitting objective is to minimize the error function: , in,( , , ) is a point The three-dimensional coordinates are subject to the following constraints: ; The normal vector of the ground plane is The vertical direction vector in the inertial coordinate system, i.e., the z-axis vector of the UAV body in the inertial coordinate system, is: Then the slope angle Normal vector with vertical direction vector The included angle is calculated using the following formula: , Preset slope angle safety threshold ,like Then keep And the pitch angle constraint of the UAV remains unchanged; if Then, the vertical coordinates of the posterior target point are adjusted using the following formula: , in, Before adjustment The vertical coordinates; The adjusted vertical coordinates; This is the vertical adjustment coefficient; Then, the pitch angle constraint is adjusted, and the upper limit of the basic pitch angle constraint is set to... Adjust the upper limit of the pitch angle constraint ,in This is the pitch angle scaling factor, and , The maximum pitch angle allowed by the mechanical structure of the drone; The adjusted posterior target point As input for subsequent posterior target releases, at the same time The data is transmitted to the underlying flight controller to constrain the drone's pitch angle range and to the posterior target point. When used as input for subsequent posterior target releases, the steps in S4 will be... Replace with ; Based on the above description, this invention calculates the slope angle by fitting the ground plane with local point cloud data, dynamically adjusts the vertical coordinates of the posterior target point and the pitch angle constraint of the UAV, accurately adapts to changes in tunnel slope, avoids instability or abnormal attitude caused by sudden changes in terrain, and enhances the adaptability of the UAV in complex terrain. It effectively covers special terrains such as uphill, downhill and inclined sections of tunnels. By predicting the slope and adjusting the attitude in a targeted manner, it avoids the collision risk caused by changes in slope in advance, reduces the flight hazards of the UAV in areas with undulating terrain, and improves the reliability of operations in difficult environments.

[0018] The embodiments of the present invention are given for illustrative and descriptive purposes only, and are not intended to be exhaustive or to limit the invention to the forms disclosed. Many modifications and variations will be apparent to those skilled in the art. The embodiments were chosen and described in order to better illustrate the principles and practical application of the invention, and to enable those skilled in the art to understand the invention and to design various embodiments with various modifications suitable for a particular purpose.

Claims

1. A method for autonomous flight planning of tunnels in local three-dimensional point cloud artificial potential fields, characterized in that: Includes the following steps: S1. Data Acquisition and Preprocessing: By subscribing to the raw point cloud data of the drone's LiDAR and the drone's current pose information in real time through the ROS interface, the point cloud data is then preprocessed to remove outliers caused by interference with the LiDAR, while simultaneously sparsening the originally high-density point cloud to reduce computational pressure. S2. Prior target generation: The maximum distance limit between the prior target and the drone is set to default to Specifically, adjustments can be made based on the actual situation to determine the centroid of the point cloud in front; S3, Artificial Potential Field Correction: Extract the radius near the prior target point The neighborhood point set is obtained and an artificial potential field is corrected. This search is completed using the kdtree module in the pcl library. S4. Post-hoc target release: Let the current position of the drone be... Then, the desired nose direction of the UAV, i.e., the x-axis vector of the UAV body in the inertial coordinate system, is set as follows: ， Let the z-axis vector of the UAV body in the vertical direction, i.e., in the inertial coordinate system, be... ,in Used to convert row vectors Convert to column vector ,but: ， in, Let y be the vector of the UAV body in the inertial coordinate system. The desired attitude of the drone is converted into a quaternion q, combined with the posterior target position, and published in PoseStamped format for execution by the underlying system.

2. The autonomous flight planning method for a local three-dimensional point cloud artificial potential field tunnel according to claim 1, characterized in that: In step S1, point cloud data preprocessing includes point cloud filtering and cropping, specifically as follows: Let the input point cloud be ,in, These are the three-dimensional coordinates of each point in the point cloud. The number of points in the point cloud is used to perform voxel filtering on the point cloud, and the voxel size is... Adjust according to the actual situation. ,in, This is the function for the voxel filtering algorithm. To input point clouds, This is the set of point clouds after voxel filtering. Voxel filtering is the process of filtering a point cloud with dimensions of 1000 x ... If a point exists in a voxel, the center of that voxel is retained as the replacement point. Then, statistical filtering is performed to remove outliers, using the following formula: ， Let the average number of neighborhoods be... That is, for each point, consider its 100 neighbors, and the standard deviation of the distances from these neighbors is... The mean distance between all points in the point cloud and their neighboring points is 1. If the average distance between this point and its neighbors is greater than If so, then discard it, and It is a point The average distance between its 100 neighboring sites, let point The neighborhood is ,but: ， in, For point To adjacent points The distance between them; After the above processing, only the forward distance of the UAV body coordinate system is retained. The points: ， in, , This is the set of valid points output after filtering.

3. The autonomous flight planning method for a local three-dimensional point cloud artificial potential field tunnel according to claim 1, characterized in that: In step S2, the formula for calculating the centroid of the foreground point cloud is as follows: ， in, For the number of points in the point cloud, Yes The average value of the points in the set, if The length is greater than Then it is necessary to Proportional shortening to Length, otherwise take the value directly. This is the prior target point.

4. The autonomous flight planning method for a local three-dimensional point cloud artificial potential field tunnel according to claim 3, characterized in that: The The formula for proportional shortening is as follows: ， in, The calculation results are for the prior target points. This is the distance scaling factor.

5. The autonomous flight planning method for a local three-dimensional point cloud artificial potential field tunnel according to claim 1, characterized in that: In step S3, the artificial potential field correction process is as follows: ， in, , For point clouds Mid-range No more than At that point, the artificial potential function is as follows: ， Then update the target point using the gradient ascent method: ， in, It is an intermediate target point in the iterative process; For the potential field function in Gradient vector at point; learning rate and maximum number of iterations Set according to the actual situation; and set constraints to limit the maximum offset of the target point, i.e. Finally, the corrected target point is obtained. ,in This is the final posterior target point 3D coordinate vector output after correction by the artificial potential field. For the first The target point's three-dimensional coordinate vector for the next iteration.

6. The autonomous flight planning method for a local three-dimensional point cloud artificial potential field tunnel according to claim 1, characterized in that: Step S3 also includes a local slope adaptive adjustment process, the specific process of which is as follows: Posterior target point corrected by artificial potential field Extract the radius centered on the radius. Point cloud set within the range , ,in, for A single three-dimensional coordinate point in the data. Extracting the radius from the point cloud for slope analysis; Using the least squares method By performing plane fitting on the point cloud, the ground plane equation is obtained: ， in, , , These are the coordinates of a point in three-dimensional space. , , normal vector of the plane The components of the plane determine its direction, where the normal vector is perpendicular to the plane; These are parameters related to the distance between the plane and the origin, used to adjust the plane's position in space; The fitting objective is to minimize the error function: ， in,( , , ) is a point The three-dimensional coordinates are subject to the following constraints: ; The normal vector of the ground plane is The vertical vector in the inertial coordinate system, i.e., the z-axis vector of the UAV body in the inertial coordinate system, is: Then the slope angle Normal vector with vertical direction vector The included angle is calculated using the following formula: 。 7. The autonomous flight planning method for a local three-dimensional point cloud artificial potential field tunnel according to claim 6, characterized in that: Preset slope angle safety threshold ,like Then keep And the pitch angle constraint of the UAV remains unchanged; if Then, the vertical coordinates of the posterior target point are adjusted using the following formula: ， in, Before adjustment The vertical coordinates; The adjusted vertical coordinates; This is the vertical adjustment coefficient; Then, the pitch angle constraint is adjusted, and the upper limit of the basic pitch angle constraint is set to... Adjust the upper limit of the pitch angle constraint ,in This is the pitch angle scaling factor, and , This is the maximum pitch angle allowed by the mechanical structure of the drone.

8. The autonomous flight planning method for a local three-dimensional point cloud artificial potential field tunnel according to claim 7, characterized in that: Adjusted posterior target point As input for subsequent posterior target releases, at the same time The data is transmitted to the underlying flight controller to constrain the drone's pitch angle range.

9. The autonomous flight planning method for a local three-dimensional point cloud artificial potential field tunnel according to claim 8, characterized in that: posterior target point When used as input for subsequent posterior target releases, the steps in S4 will be... Replace with .

Images