Mine unmanned aerial vehicle path planning method based on adaptive sampling and safety distance constraint

Abstract

The invention discloses a mine unmanned aerial vehicle path planning method based on adaptive sampling and safety distance constraint. Firstly, a roadway environment map is constructed, and roadway local safety distance information is obtained through distance transformation; then, path searching is carried out based on the target distance adaptive bias probability and the safe distance adaptive step length, the target sampling probability is dynamically adjusted according to the distance between a node and a target point, and the expansion step length is adaptively adjusted in combination with the local safe distance, so that rapid convergence and safe expansion of the searching process are realized; and finally, path reverse optimization is carried out, a cubic B-spline trajectory optimization model introducing a safety distance penalty term is constructed, and a continuous executable trajectory satisfying unmanned aerial vehicle speed, acceleration and safety distance constraints is generated. Experimental results show that the number of search nodes and the search time are remarkably reduced on the premise that the path quality is guaranteed, and the path planning efficiency and the flight safety of the unmanned aerial vehicle in long and narrow limited environments such as mine laneways are improved.

Description

Technical Field

[0001] This invention relates to the field of autonomous navigation and path planning technology for unmanned aerial vehicles (UAVs), particularly a path planning method for UAVs in mine roadways. Specifically, it relates to an adaptive sampling and safe distance constraint-based path planning method for UAVs in mines. Background Technology

[0002] With the development of UAV technology, the demand for its application in underground space operations such as mine inspection, disaster exploration, and environmental monitoring is increasing. Mine tunnel environments are typically characterized by narrow spaces, limited access areas, and complex obstacle distribution, placing high demands on the autonomous navigation and path planning capabilities of UAVs. Among existing UAV path planning methods, the Rapidly-exploring Random Tree (RRT) algorithm is widely used in path planning in complex environments due to its probabilistic completeness and strong global search capabilities. However, traditional RRT algorithms typically employ fixed step sizes and uniform random sampling strategies, failing to fully utilize prior information such as the distribution of accessible space and safe distances in mine tunnel environments. This can easily lead to a large number of invalid extensions in narrow environments, resulting in low search efficiency. Furthermore, traditional path planning methods often focus on geometric feasibility, generating paths that typically consist of a large number of discrete nodes, resulting in clearly defined path polygons. This makes it difficult to directly meet the dynamic constraints of UAVs in terms of speed, acceleration, and safe distances, usually requiring additional trajectory smoothing and optimization processing. Existing trajectory optimization methods either have high computational complexity or rely on complex models, making them difficult to implement in engineering and thus hard to apply in real-world scenarios such as mines. Therefore, there is a need for a UAV path planning and trajectory optimization method that is structurally clear, easy to implement, and can simultaneously balance path search efficiency, flight safety, and dynamic feasibility.

[0003] Invention patent CN113359775A discloses a dynamic variable sampling area (RRT) unmanned vehicle path planning method. First, map information is initialized, and the current area is determined according to the dynamic variable sampling area formula. Based on this, collision detection with a reserved safety distance is performed. New nodes are generated according to the probability target bias formula and step size selection formula. A reverse search is performed to output the path. Finally, considering the maximum turning angle constraint, the output path is optimized through reverse optimization and cubic B-spline curve fitting. However, the following problems exist: This patent uses a fixed probability when targeting, which easily leads to high probability bias towards the target in the early planning stage, resulting in frequent collisions with obstacles and affecting search efficiency. Furthermore, the safety distance is not considered during cubic B-spline optimization, which easily leads to the optimized path closely following obstacles.

[0004] The paper "Improved RRT Algorithm for Robotic Arm Path Planning Based on Dynamic Probability Sampling" proposes an improved RRT algorithm based on dynamic probability sampling. First, it reflects the changes in local obstacles in the environment by accumulating the number of collisions during node expansion and dynamically adjusts the sampling bias probability of the target point accordingly. Second, it introduces an adaptive step-size strategy, adjusting the node expansion step size based on the relative distance between random points and the target point. Then, it limits the sampling area by limiting the expansion angle to reduce invalid expansion. Finally, it uses cubic B-spline curves to smooth the planned path, thereby improving the continuity and executability of the path. However, the following problems exist: the dynamic probability sampling in this paper... The mechanism uses the cumulative number of collisions during node expansion as a basis for reflecting environmental complexity. Only after multiple collisions do the target bias probability change significantly. This approach is difficult to reflect changes in the search phase when the environment changes or approaches the target area. It is prone to maintaining high randomness in narrow passages or later search phases, thus affecting the search convergence speed. Furthermore, the step size adjustment in this method is mainly based on the relative distance between the node and the target point, without explicitly considering the size of the passable space around the node or the safe distance from obstacles. In narrow environments, the step size may still be mismatched with the local spatial scale, affecting the stability and safety of the path search.

[0005] The paper "An Improved RRT Algorithm Based on Kinematic Constraints" addresses the problem that traditional RRT algorithms often fail to meet kinematic constraints in generating paths. It proposes an improved RRT path planning method that incorporates kinematic constraints. This method first introduces vehicle kinematic model constraints during the node expansion stage, ensuring that newly generated nodes geometrically meet kinematic feasibility requirements such as minimum turning radius. Second, it reduces the generation of unexecutable paths by constraining node posture and direction of motion during expansion. Finally, it performs path smoothing on the planning results to improve path executability. While this method shows some improvement in introducing kinematic constraints, it still has the following shortcomings: It focuses primarily on whether the path meets kinematic constraints, but does not incorporate safe distance information between the path and obstacles in the environment during path search. Node expansion is still mainly based on geometric feasibility judgments. In narrow, confined mine tunnel environments, this approach struggles to dynamically adjust the search strategy according to the size of the local passage space, easily leading to inefficient expansion behavior in narrow areas. Furthermore, the path smoothing in this paper mainly improves geometric continuity, lacking a further optimization mechanism for the safety margin between the path and obstacles, making it difficult to actively balance path smoothness and safety. Summary of the Invention

[0006] To address the problems of existing technologies, this invention proposes a path planning method for unmanned aerial vehicles (UAVs) in mine roadways based on adaptive sampling and step size adjustment. First, the mine roadway environment is constructed as a grid map, and local safety distance information is obtained through distance transformation. Based on this, a fast random tree path planning model is built. During the path search phase, an adaptive target bias sampling probability based on target distance and an adaptive expansion step size based on safety distance are introduced to improve path search efficiency and safety. After obtaining an initial feasible path, the path is simplified and smoothed using a path backward optimization method. Finally, a cubic B-spline trajectory optimization method that incorporates UAV dynamic constraints and safety distance penalties is used to generate a continuous flight trajectory that satisfies the UAV's speed, acceleration, and safety distance constraints.

[0007] To achieve the above objectives, the present invention employs the following specific technical solutions:

[0008] 1. A path planning method for unmanned aerial vehicles (UAVs) in mines with adaptive sampling and safety distance constraints, characterized by comprising the following steps:

[0009] S1: Initialize map information, perform distance transformation and safety margin modeling on the tunnel map, which includes the following sub-steps:

[0010] S1.1: The actual mine roadway environment is discretized and constructed as a two-dimensional occupied grid map M, which can be represented as a map of size N. x ×N y A two-dimensional matrix, where each grid cell corresponds to a region of equal size in the actual environment. Let any grid cell be denoted as:

[0011] m(i,j), i∈[1,N] x ],j∈[1,N y (1)

[0012] In the formula, i represents the row index of the raster in the map, j represents the column index of the raster in the map, and N... x N y These represent the number of grid cells in the row and column directions of the map, respectively. Based on the distribution of obstacles (such as tunnel walls, support structures, etc.) and passable areas in the mine tunnel environment, each grid cell is assigned an occupancy status value, defined as follows: if it is an obstacle, the corresponding array is set to 1; if it is a feasible path, the corresponding array is set to 0.

[0013] S1.2: Based on the two-dimensional occupied grid map M, perform distance transformation operation on all free grids to obtain the distance transformation matrix D. The distance transformation function is defined as:

[0014]

[0015] In the formula, (x,y) represents the spatial coordinates of the current free grid cell, (x0,y0) represents the spatial coordinates of the obstacle grid cell, and D(x,y) represents the minimum Euclidean distance from the current position to the nearest roadway wall or obstacle. This distance transformation value is used to characterize the local passage width and safety margin information of the mine roadway at different locations. The larger the distance value, the greater the safe flight space of the UAV in the area. After the distance transformation is completed, the distance values ​​of all passable grid cells are uniformly stored in the distance transformation matrix D. Each element in the matrix corresponds one-to-one with the corresponding grid cell in the grid map.

[0016] S2: Perform fast random tree initialization, set the start node and target node, and set the planning parameters. This includes the following sub-steps:

[0017] S2.1: Based on the initial takeoff position of the UAV and the mission target position, determine the starting node and target node respectively in the grid map. The starting node is defined as:

[0018] X start =(x s ,y s (3)

[0019] In the formula, (x s ,y s () represents the coordinates of the drone's starting position in the grid map, and the target node is defined as:

[0020] X goal =(x g ,y g (4)

[0021] In the formula, (x g ,y g The coordinates of the UAV target location in the grid map are represented by the starting node and the target node, which are both located within the passable grid cells.

[0022] S2.2: Starting with node X start As the root node of the fast random tree, initialize the fast random tree structure T, and add the starting node to the tree structure: T = {X} start Meanwhile, the index information of the parent node is maintained for each node in the tree, which is used for path backtracking after a successful path search.

[0023] S2.3: Before path planning begins, initialize the relevant parameters of the fast random tree algorithm, including: maximum number of iterations N. max This is used to limit the maximum number of calculations for path searching. To improve the robustness of path searching and avoid requiring the path to precisely reach a single grid point, r is set.g Indicates the radius of the target area.

[0024] S3: After completing the mine roadway environment modeling and fast random tree initialization, two adjustment mechanisms are introduced to optimize the path search process: adaptive target bias probability based on target distance and adaptive expansion step size based on safety distance. By introducing probability adjustment related to target distance in the sampling phase and step size adjustment related to the local safety distance of the roadway in the expansion phase, the path search process can simultaneously consider global exploration capability, target convergence speed, and expansion safety. Specifically, it includes the following sub-steps:

[0025] S3.1: Randomly generate sampling points X rand Find the distance of random sampling point X in random tree T. rand The nearest node is taken as the nearest point X. near .

[0026] S3.2: Calculate the current nearest point X near With target node X goal Euclidean distance:

[0027] d(X near )=||X near -X goal || (5)

[0028] In the formula, ||·|| represents the Euclidean norm, which is used to characterize the proximity of the current search state to the target position. The target bias sampling probability is then:

[0029] P(X near ) = P min +(P max -P min )exp(-γd(X near (6)

[0030] In the formula, P min P represents the minimum target bias probability, used to ensure sufficient exploration in the early stages of the search. max The maximum value of the target bias probability is used to enhance target guidance in the later stages of the search. γ is the target range attenuation coefficient, used to adjust the rate of probability change. near When the value is large, the target bias probability approaches P. min When d(X) near When the value is small, the target bias probability gradually increases and approaches P. max .

[0031] S3.3: Constructing the random point reconstruction function:

[0032]

[0033] In the formula, D(X) rand () represents a random point X rand The minimum Euclidean distance, D, to the nearest tunnel wall or obstacle min Let ω be the target weighting factor, and r be a random number in the range [0,1]. Then, the random point X is calculated according to the formula. rand Make a new selection.

[0034] S3.4: Based on the distance transformation result of step S1, obtain the nearest node X. near The safe distance D(X) at the location near This safety distance represents the minimum Euclidean distance from the node to the nearest roadway wall or obstacle. An adaptive expansion step size function is constructed based on this safety distance:

[0035] δ(X near )=min(δ min +k·D(X near ),δ max (8)

[0036] In the formula, δ min δ represents the minimum expansion step size. max This represents the maximum expansion step size, and k is the step size adjustment coefficient.

[0037] S3.5: Expand the new node, with the expansion direction unit vector as follows:

[0038]

[0039] In the formula, u represents the direction from the nearest node to the random sampling point, and the new node is generated in the following way:

[0040] X new =X near +δ(X near )·u (10)

[0041] S3.6: Before adding a new node to the fast random tree, perform collision detection on the expansion path based on a safe distance threshold. If the expansion path collides with a road wall or obstacle, the expansion is considered a failure; otherwise, the expansion is considered successful, and the new node X is added. near Add the node to the fast random tree T and record its parent node as X. near Repeat the above steps until a newly generated node satisfies |X goal -X new |≤r g Or it may reach the maximum number of iterations.

[0042] S4: Obtain the initial path using a fast random tree, perform reverse optimization on the initial path, and consider UAV dynamic constraints and safe distances to perform cubic B-spline optimization to ensure the feasibility of the path. This includes the following sub-steps:

[0043] S4.1: After completing the path search based on a fast random tree, when the search tree first reaches the target region, an initial path consisting of discrete nodes is obtained by backtracking the parent node relationship from the target node to the starting node:

[0044] X = {X start ,X1, ...,X goal} (11)

[0045] In the formula, X i Let be the i-th discrete node in the path, and n represent the total number of nodes in the initial path. Although the initial path can guarantee connectivity from the starting point to the target point, it usually has problems such as a large number of nodes, obvious path polylines, and frequent turns, making it difficult to directly meet the dynamic constraints of the UAV.

[0046] S4.2: Perform reverse path optimization, first connect X start With X goal Form an initial path. If there are no obstacles on the path, then this path is considered the optimal path. If there are obstacles, then connect X. goal The previous node and X start Determine if there are obstacles in the path. If there are no obstacles, then set X... start As the parent node of the current node, and starting from the current node, repeat the above steps until the target node is reached, complete the path optimization, and save the optimized path.

[0047] S4.3: After completing the reverse path optimization, an optimized path consisting of a finite number of key nodes is obtained. Although this path geometrically satisfies connectivity and safety, it is still a discrete polyline path, which is difficult to meet the flight control requirements of the UAV in terms of velocity and acceleration continuity. Therefore, using the path nodes after reverse optimization as input, the path is made continuous and smoothed using a cubic B-spline curve to generate a continuous flight trajectory that satisfies the dynamic constraints of the UAV. The path nodes after reverse optimization are used as the control point set of the cubic B-spline curve, and the parameterized representation of the cubic B-spline curve is as follows:

[0048]

[0049] In the formula, N i,3 (t) represents the cubic B-spline basis function, P i Let t0,t be the control points. fThe parameters are the trajectory start and end points, respectively. K is the number of control points. The first and second derivatives of the trajectory represent the velocity and acceleration vectors of the UAV, respectively. The UAV dynamics and safety constraints are then constructed.

[0050]

[0051] In the formula, v max a is the maximum permissible flight speed for a drone. max To determine the maximum allowable acceleration for the drone and to further enhance the safety margin between the trajectory and the tunnel wall during trajectory optimization, a safety distance penalty function is introduced:

[0052] φ(D(r(t)))=max(0,D min -D(r(t))) (14)

[0053] When a trajectory point approaches or violates the safety distance threshold, the penalty function value increases with the degree of safety distance violation. Considering both trajectory smoothness and flight safety, a comprehensive trajectory optimization objective function is constructed:

[0054]

[0055] In the formula, a1, a2, and a3 are weighting coefficients used to balance the requirements of smoothness, controllability, and safety, and to minimize the objective function to generate the final trajectory.

[0056] The present invention has the following beneficial effects:

[0057] 1. This invention introduces an adaptive bias sampling probability based on target distance and an adaptive expansion step size strategy based on safety distance in the fast random tree path search process. This allows the search process to dynamically adjust sampling and expansion behavior according to the search stage and local access space characteristics. Experimental results show that, in the same mine roadway environment, the algorithm of this invention generates 402 search nodes, which is about 75% less than the 1606 nodes generated by the original RRT algorithm. The search time is reduced from 17.76s to 1.73s, a reduction of about 90%. This invention can effectively reduce the expansion of invalid nodes in narrow roadway environments and significantly improve path search efficiency and convergence speed.

[0058] 2. To address the problem that traditional path planning results are difficult to directly apply to actual UAV flights, this invention introduces a path backward optimization method and a cubic B-spline trajectory optimization method that incorporates UAV dynamic constraints and safety distance penalties after obtaining the initial path. Experimental results show that after backward optimization, the path length is reduced from the original path to 857m, a reduction of approximately 13.8%, and the number of turns in the path is significantly reduced. After cubic B-spline optimization, the path length is further optimized to 848m, a reduction of approximately 14.7% compared to the original path, and the path is continuous and smooth overall, meeting the actual flight requirements of UAVs in terms of speed, acceleration, and safety distance, thereby significantly improving the feasibility and flight safety of the path. Attached Figure Description

[0059] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0060] Figure 1 This is a flowchart illustrating the overall process of the present invention.

[0061] Figure 2 This is a small quadcopter drone used in this experiment;

[0062] Figure 3 This refers to the map acquisition environment for this experiment;

[0063] Figure 4 The original RRT algorithm searches the node graph;

[0064] Figure 5 The goal-biased RRT algorithm searches the node graph;

[0065] Figure 6 The DVSA-RRT algorithm searches the node graph;

[0066] Figure 7 This is the search node graph for the algorithm of this invention;

[0067] Figure 8 Optimize the path graph for cubic B-spline without considering safety distance penalties;

[0068] Figure 9 This is the path diagram for the cubic B-spline optimization of this invention. Detailed Implementation

[0069] To make the above-mentioned objectives, features, and advantages of the present invention more apparent and understandable, a method for UAV path planning in mine roadways based on adaptive sampling and step size adjustment is provided, such as... Figure 1 As shown, it includes the following steps:

[0070] S1: Initialize map information, perform distance transformation and safety margin modeling on the tunnel map, which includes the following sub-steps:

[0071] S1.1: The actual mine roadway environment is discretized and constructed as a two-dimensional occupied grid map M, which can be represented as a map of size N. x ×N y A two-dimensional matrix, where each grid cell corresponds to a region of equal size in the actual environment. Let any grid cell be denoted as:

[0072] m(i,j), i∈[1,N] x ],j∈[1,N y (1)

[0073] In the formula, i represents the row index of the raster in the map, j represents the column index of the raster in the map, and N... x N y These represent the number of grid cells in the row and column directions of the map, respectively. Based on the distribution of obstacles (such as tunnel walls, support structures, etc.) and passable areas in the mine tunnel environment, each grid cell is assigned an occupancy status value, defined as follows: if it is an obstacle, the corresponding array is set to 1; if it is a feasible path, the corresponding array is set to 0.

[0074] S1.2: Based on the two-dimensional occupied grid map M, perform distance transformation operation on all free grids to obtain the distance transformation matrix D. The distance transformation function is defined as:

[0075]

[0076] In the formula, (x,y) represents the spatial coordinates of the current free grid cell, (x0,y0) represents the spatial coordinates of the obstacle grid cell, and D(x,y) represents the minimum Euclidean distance from the current position to the nearest roadway wall or obstacle. This distance transformation value is used to characterize the local passage width and safety margin information of the mine roadway at different locations. The larger the distance value, the greater the safe flight space of the UAV in the area. After the distance transformation is completed, the distance values ​​of all passable grid cells are uniformly stored in the distance transformation matrix D. Each element in the matrix corresponds one-to-one with the corresponding grid cell in the grid map.

[0077] S2: Perform fast random tree initialization, set the start node and target node, and set the planning parameters. This includes the following sub-steps:

[0078] S2.1: Based on the initial takeoff position of the UAV and the mission target position, determine the starting node and target node respectively in the grid map. The starting node is defined as:

[0079] X start =(x s ,y s (3)

[0080] In the formula, (x s ,y s () represents the coordinates of the drone's starting position in the grid map, and the target node is defined as:

[0081] X goal =(x g ,y g (4)

[0082] In the formula, (x g ,y g The coordinates of the UAV target location in the grid map are represented by the starting node and the target node, which are both located within the passable grid cells.

[0083] S2.2: Starting with node X start As the root node of the fast random tree, initialize the fast random tree structure T, and add the starting node to the tree structure: T = {X} start Meanwhile, the index information of the parent node is maintained for each node in the tree, which is used for path backtracking after a successful path search.

[0084] S2.3: Before path planning begins, initialize the relevant parameters of the fast random tree algorithm, including: maximum number of iterations N. max This is used to limit the maximum number of calculations for path searching. To improve the robustness of path searching and avoid requiring the path to precisely reach a single grid point, r is set. g Indicates the radius of the target area.

[0085] S3: After completing the mine roadway environment modeling and fast random tree initialization, two adjustment mechanisms are introduced to optimize the path search process: adaptive target bias probability based on target distance and adaptive expansion step size based on safety distance. By introducing probability adjustment related to target distance in the sampling phase and step size adjustment related to the local safety distance of the roadway in the expansion phase, the path search process can simultaneously consider global exploration capability, target convergence speed, and expansion safety. Specifically, it includes the following sub-steps:

[0086] S3.1: Randomly generate sampling points X rand Find the distance of random sampling point X in random tree T. rand The nearest node is taken as the nearest point X. near .

[0087] S3.2: Calculate the current nearest point X near With target node X goal Euclidean distance:

[0088] d(X near )=||X near -X goal || (5)

[0089] In the formula, ||·|| represents the Euclidean norm, which is used to characterize the proximity of the current search state to the target position. The target bias sampling probability is then:

[0090] P(X near ) = P min +(P max -P min )exp(-γd(X near (6)

[0091] In the formula, P min P represents the minimum target bias probability, used to ensure sufficient exploration in the early stages of the search. max The maximum value of the target bias probability is used to enhance target guidance in the later stages of the search. γ is the target range attenuation coefficient, used to adjust the rate of probability change. near When the value is large, the target bias probability approaches P. min When d(X) near When the value is small, the target bias probability gradually increases and approaches P. max .

[0092] S3.3: Constructing the random point reconstruction function:

[0093]

[0094] In the formula, D(X) rand () represents a random point X rand The minimum Euclidean distance, D, to the nearest tunnel wall or obstacle min Let ω be the target weighting factor, and r be a random number in the range [0,1]. Then, the random point X is calculated according to the formula. rand Make a new selection.

[0095] S3.4: Based on the distance transformation result of step S1, obtain the nearest node X. near The safe distance D(X) at the location near This safety distance represents the minimum Euclidean distance from the node to the nearest roadway wall or obstacle. An adaptive expansion step size function is constructed based on this safety distance:

[0096] δ(X near )=min(δmin +k·D(X near ),δ max (8)

[0097] In the formula, δ min δ represents the minimum expansion step size. max This represents the maximum expansion step size, and k is the step size adjustment coefficient.

[0098] S3.5: Expand the new node, with the expansion direction unit vector as follows:

[0099]

[0100] In the formula, u represents the direction from the nearest node to the random sampling point, and the new node is generated in the following way:

[0101] X new =X near +δ(X near )·u (10)

[0102] S3.6: Before adding a new node to the fast random tree, perform collision detection on the expansion path based on a safe distance threshold. If the expansion path collides with a road wall or obstacle, the expansion is considered a failure; otherwise, the expansion is considered successful, and the new node X is added. near Add the node to the fast random tree T and record its parent node as X. near Repeat the above steps until a newly generated node satisfies |X goal -X new |≤r g Or it may reach the maximum number of iterations.

[0103] S4: Obtain the initial path using a fast random tree, perform reverse optimization on the initial path, and consider UAV dynamic constraints and safe distances to perform cubic B-spline optimization to ensure the feasibility of the path. This includes the following sub-steps:

[0104] S4.1: After completing the path search based on a fast random tree, when the search tree first reaches the target region, an initial path consisting of discrete nodes is obtained by backtracking the parent node relationship from the target node to the starting node:

[0105] X = {X start ,X1,...,X goal} (11)

[0106] In the formula, X i Let be the i-th discrete node in the path, and n represent the total number of nodes in the initial path. Although the initial path can guarantee connectivity from the starting point to the target point, it usually has problems such as a large number of nodes, obvious path polylines, and frequent turns, making it difficult to directly meet the dynamic constraints of the UAV.

[0107] S4.2: Perform reverse path optimization, first connect X start With X goal Form an initial path. If there are no obstacles on the path, then this path is considered the optimal path. If there are obstacles, then connect X. goal The previous node and X start Determine if there are obstacles in the path. If there are no obstacles, then set X... start As the parent node of the current node, and starting from the current node, repeat the above steps until the target node is reached, complete the path optimization, and save the optimized path.

[0108] S4.3: After completing the reverse path optimization, an optimized path consisting of a finite number of key nodes is obtained. Although this path geometrically satisfies connectivity and safety, it is still a discrete polyline path, which is difficult to meet the flight control requirements of the UAV in terms of velocity and acceleration continuity. Therefore, using the path nodes after reverse optimization as input, the path is made continuous and smoothed using a cubic B-spline curve to generate a continuous flight trajectory that satisfies the dynamic constraints of the UAV. The path nodes after reverse optimization are used as the control point set of the cubic B-spline curve, and the parameterized representation of the cubic B-spline curve is as follows:

[0109]

[0110] In the formula, N i,3 (t) represents the cubic B-spline basis function, P i Let t0,t be the control points. f The parameters are the trajectory start and end points, respectively. K is the number of control points. The first and second derivatives of the trajectory represent the velocity and acceleration vectors of the UAV, respectively. The UAV dynamics and safety constraints are then constructed.

[0111]

[0112] In the formula, v max a is the maximum permissible flight speed for a drone. max To determine the maximum allowable acceleration for the drone and to further enhance the safety margin between the trajectory and the tunnel wall during trajectory optimization, a safety distance penalty function is introduced:

[0113] φ(D(r(t)))=max(0,D min -D(r(t))) (14)

[0114] When a trajectory point approaches or violates the safety distance threshold, the penalty function value increases with the degree of safety distance violation. Considering both trajectory smoothness and flight safety, a comprehensive trajectory optimization objective function is constructed:

[0115]

[0116] In the formula, a1, a2, and a3 are weighting coefficients used to balance the requirements of smoothness, controllability, and safety. The objective function is minimized to generate the final trajectory. The overall algorithm flowchart is as follows: Figure 1 As shown.

[0117] Experimental verification of the algorithm of this invention includes comparative experiments with the original RRT algorithm, the goal-biased RRT algorithm, the DVSA-RRT algorithm, and this method. The specific steps are as follows:

[0118] Set the maximum number of samples N max =3000, endpoint distance threshold r g =40, minimum goal paranoia probability P min =0.1, maximum goal paranoia probability P max =0.8, target distance attenuation coefficient γ =0.01, safety threshold D min =5, target weight factor ω = 0.5, step size adjustment coefficient k = 1.5, minimum step size δ min =5, maximum step size δ max =30.

[0119] The software environment for the experiment was Matlab 2023b, and the hardware environment was a 13th Gen Intel Core i9-13900HX + NVIDIA GeForce RTX 4060 + 16GB of memory.

[0120] This experiment used a small quadcopter drone to collect environmental data from mine tunnels. The small quadcopter drone used was, for example... Figure 2 As shown, the core control unit of the drone uses the Pixhawk second-generation flight controller. To enhance the drone's computing power and processing speed, an NVIDIA JETSON Xavier NX onboard computer is installed beneath the drone platform. For sensor configuration, an Intel RealSense D435i camera is mounted at the front of the drone, consisting of one RGB camera, two infrared cameras, an infrared transmitter, and an IMU unit. The experimental environment is as follows. Figure 3 As shown, a D435i camera carried in front of the drone was used to acquire a map of the mine tunnels. The tunnel map was then rasterized. Each algorithm was tested 50 times. The experimental results are shown in Table 1.

[0121] Table 1 Experimental Results

[0122]

[0123] As shown in Table 1, the original RRT algorithm generates the most nodes during the search process, totaling 1606, indicating a large number of ineffective expansions in narrow alleyway environments. After introducing target bias sampling, the number of search nodes decreases to 1124, demonstrating that the target bias strategy guides the search direction to some extent and reduces randomness. The DVSA-RRT algorithm further reduces the number of search nodes to 672 through a sampling mechanism related to variable sampling areas and probabilistic target bias. Furthermore, the algorithm of this invention combines a target distance adaptive bias probability and a safety distance adaptive step size strategy, further reducing the number of search nodes to 402, a reduction of approximately 75% compared to the original RRT algorithm, significantly improving search efficiency. The search time of the original RRT algorithm is 17.76 s, the target bias RRT algorithm shortens the search time to 9.46 s, the DVSA-RRT algorithm further shortens it to 3.65 s, while the search time of the algorithm of this invention is only 1.73 s, a reduction of approximately 90% compared to the original RRT algorithm. This demonstrates that the target distance adaptive bias sampling and safety distance adaptive step size strategy proposed in this invention has significant advantages in reducing invalid expansion and accelerating search convergence. The search node graphs of the various comparative algorithms are shown below. Figure 4 , Figure 5 , Figure 6 , Figure 7 As shown, this method requires the fewest search nodes and converges quickly in the region close to the target point. Figure 8 The optimized path graph for cubic B-spline without considering safety distance penalties shows that the path conforms to the obstacles and cannot meet the safety requirements for drone flight. Figure 9 The path diagram after cubic B-spline optimization proposed in this invention shows that the distance between the path and the tunnel wall meets the requirements for safe flight of the UAV. The path length after cubic B-spline optimization is 848m, which is 14.7% shorter than the original path, and the path is smoother, meeting the actual flight requirements of the UAV.

[0124] The experimental results above show that the algorithm of this invention significantly reduces the number of search nodes and search time, generates smoother paths, and significantly improves the path planning efficiency and engineering practicality of UAVs in narrow and confined environments such as mine tunnels.

[0125] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the technical solutions of the present invention have been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the present invention, and all such modifications or substitutions should be covered within the protection scope of the present invention.

Claims

1. A path planning method for unmanned aerial vehicles (UAVs) in mines with adaptive sampling and safe distance constraints, characterized in that, Includes the following steps: S1: Initialize map information, perform distance transformation and safety margin modeling on the tunnel map, which includes the following sub-steps: S1.1: Discretize the actual mine roadway environment and construct a two-dimensional occupied grid map M; S1.2: Based on the two-dimensional occupied grid map M, perform distance transformation operation on all free grids to obtain the distance transformation matrix D. The distance transformation function is defined as: In the formula, (x,y) represents the spatial coordinates of the current free grid cell, (x0,y0) represents the spatial coordinates of the obstacle grid cell, and D(x,y) represents the minimum Euclidean distance from the current position to the nearest roadway wall or obstacle. This distance transformation value is used to characterize the local passage width and safety margin information of the mine roadway at different locations. The larger the distance value, the greater the safe flight space of the UAV in the area. After the distance transformation is completed, the distance values ​​of all passable grid cells are uniformly stored in the distance transformation matrix D. Each element in the matrix corresponds one-to-one with the corresponding grid cell in the grid map. S2: Perform fast random tree initialization, set the starting node and target node, and set the planning parameters. This includes the following sub-steps: S2.1: Based on the initial takeoff position of the UAV and the position of the mission target, determine the starting node and the target node in the grid map respectively; S2.2: Starting with node X start As the root node of the fast random tree, initialize the fast random tree structure T, and add the starting node to the tree structure: T = {X} start Meanwhile, the parent node index information is maintained for each node in the tree, which is used for path backtracking after a successful path search. S2.3: Before path planning begins, initialize the relevant parameters of the fast random tree algorithm, including: maximum number of iterations N. max This is used to limit the maximum number of calculations for path searching. To improve the robustness of path searching and avoid requiring the path to precisely reach a single grid point, r is set. g Indicates the radius of the target area; S3: After completing the mine roadway environment modeling and fast random tree initialization, two adjustment mechanisms are introduced to optimize the path search process: adaptive target bias probability based on target distance and adaptive expansion step size based on safety distance. By introducing probability adjustment related to target distance in the sampling phase and step size adjustment related to the local safety distance of the roadway in the expansion phase, the path search process can simultaneously take into account global exploration capability, target convergence speed, and expansion safety. Specifically, it includes the following sub-steps: S3.1: Randomly generate sampling points X rand Find the distance X from the random sampling point in the random tree T. rand The nearest node is taken as the nearest point X. near ; S3.2: Calculate the current nearest point X near With target node X goal Euclidean distance: d(X near )=||X near -X goal || (2) In the formula, ||·|| represents the Euclidean norm, which is used to characterize the proximity of the current search state to the target position. The target bias sampling probability is then: P(X near )=P min +(P max -P min )exp(-γd(X near )) (3) In the formula, P min P represents the minimum target bias probability, used to ensure sufficient exploration in the early stages of the search. max The maximum value of the target bias probability is used to enhance target guidance in the later stages of the search. γ is the target range attenuation coefficient, used to adjust the rate of probability change. near When the value is large, the target bias probability approaches P. min When d(X) near When the value is small, the target bias probability gradually increases and approaches P. max ; S3.3: Constructing a random point reconstruction function: In the formula, D(X) rand () represents a random point X rand The minimum Euclidean distance, D, to the nearest tunnel wall or obstacle min Let ω be the target weighting factor, and r be a random number in the range [0,1]. Then, the random point X is calculated according to the formula. rand Make a new selection; S3.4: Based on the distance transformation result of step S1, obtain the nearest node X. near The safe distance D(X) at the location near The safety distance represents the minimum Euclidean distance from the node to the nearest roadway wall or obstacle. An adaptive expansion step size function is constructed based on the safety distance. δ(X near )=min(δ min +k·D(X near ),d max ) (5) In the formula, δ min δ represents the minimum expansion step size. max This represents the maximum expansion step size, where k is the step size adjustment coefficient; S3.5: Expand the new node, with the expansion direction unit vector as follows: In the formula, u represents the direction from the nearest node to the random sampling point, and the new node is generated in the following way: X new =X near +δ(X near )·u (7) S3.6: Before adding a new node to the fast random tree, perform collision detection on the expansion path based on a safe distance threshold. If the expansion path collides with a road wall or obstacle, the expansion is considered a failure; otherwise, the expansion is considered successful, and the new node X is added. near Add the node to the fast random tree T and record its parent node as X. near Repeat the above steps until a newly generated node satisfies |X goal -X new |≤r g Or reach the maximum number of iterations; S4: Obtain the initial path using a fast random tree, perform reverse optimization on the initial path, and consider UAV dynamic constraints and safe distances to perform cubic B-spline optimization to ensure the feasibility of the path. This includes the following sub-steps: S4.1: After completing the path search based on the fast random tree, when the search tree reaches the target area for the first time, an initial path composed of discrete nodes is obtained by backtracking the parent node relationship from the target node to the starting node. S4.2: Perform reverse path optimization, first connect X start With X goal Form an initial path. If there are no obstacles on the path, then this path is considered the optimal path. If there are obstacles, then connect X. goal The previous node and X start Determine if there are obstacles in the path. If there are no obstacles, then set X... start As the parent node of the current node, and starting from the current node, repeat the above steps until the target node is reached, complete the path optimization and save the optimized path; S4.3: After completing the reverse path optimization, an optimized path consisting of a finite number of key nodes is obtained. Using the path nodes after reverse optimization as input, the path is made continuous and smoothed through cubic B-spline curves to generate a continuous flight trajectory that satisfies the dynamic constraints of the UAV. The path nodes after reverse optimization are used as the control point set of the cubic B-spline curve. The parameterized representation of the cubic B-spline curve is as follows: In the formula, N i,3 (t) represents the cubic B-spline basis function, P i Let t0,t be the control points. f The parameters are the trajectory start and end points, respectively. K is the number of control points. The first and second derivatives of the trajectory represent the velocity and acceleration vectors of the UAV, respectively. The UAV dynamics and safety constraints are then constructed. In the formula, v max a is the maximum permissible flight speed for a drone. max To determine the maximum allowable acceleration for the drone and to further enhance the safety margin between the trajectory and the tunnel wall during trajectory optimization, a safety distance penalty function is introduced: φ(D(r(t)))=max(0,D min -D(r(t))) (10) When a trajectory point approaches or violates the safety distance threshold, the penalty function value increases with the degree of safety distance violation. Considering both trajectory smoothness and flight safety, a comprehensive trajectory optimization objective function is constructed: In the formula, a1, a2, and a3 are weighting coefficients used to balance the requirements of smoothness, controllability, and safety, and to minimize the objective function to generate the final trajectory.

Images