An unmanned aerial vehicle obstacle avoidance trajectory planning method with tangent line smoothing constraint

Abstract

The application discloses a kind of unmanned aerial vehicle obstacle avoidance trajectory planning methods with tangent line smooth constraint, to solve the problem that trajectory smoothness, obstacle avoidance safety and real-time performance are difficult to consider in complex three-dimensional environment.The method is first to three-dimensional discrete modeling to flight space, constructs local visible area and generates target-oriented trajectory;When detecting potential collision risk, construct navigation plane perpendicular to flight direction at first collision point, project obstacle to two-dimensional plane, extract boundary contour and tangent point, form candidate tangent direction.Adaptive sampling is carried out along the tangent direction combined with kinematic constraint, and continuous feasible candidate waypoints are generated.Subsequently, an evaluation function is constructed by combining angle deviation and safety distance, and the optimal waypoint is selected for iterative updating until the target is reached.Compared with the prior art, the application can generate smooth and continuous unmanned aerial vehicle trajectory in dense obstacle environment, realize smooth, safe and real-time three-dimensional obstacle avoidance flight.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) trajectory planning technology, and more particularly to a UAV obstacle avoidance trajectory planning method with tangent smoothness constraints. This method can generate safe, smooth, and dynamically feasible flight trajectories in real time in complex three-dimensional environments, and is suitable for environments with dense or dynamically changing obstacles, improving the responsiveness and efficiency of autonomous UAV flight. Background Technology

[0002] Unmanned aerial vehicles (UAVs) are increasingly used in environmental monitoring, logistics transportation, rescue missions, and military reconnaissance, with their autonomous flight capabilities becoming a crucial guarantee for accomplishing complex tasks. Trajectory planning is one of the core technologies for autonomous UAV flight, aiming to generate the optimal flight path from the starting point to the target point while ensuring safety and dynamic feasibility. However, in complex three-dimensional environments, especially under conditions of densely distributed obstacles, irregular shapes, or dynamically changing environments, traditional trajectory planning methods face multiple challenges.

[0003] Existing trajectory planning methods mainly include graph search-based strategies, sampling optimization, and local obstacle avoidance. Graph search-based methods achieve global shortest path search by discretizing the 3D space and constructing a grid or graph structure. However, they are computationally intensive, making it difficult to meet the real-time planning needs of UAVs in complex environments. Furthermore, the generated paths are often polygonal, resulting in poor smoothness and dynamic feasibility. Sampling optimization methods, such as fast random trees or probabilistic road maps, demonstrate strong adaptability to local environments and can quickly generate feasible trajectories. However, they are prone to unnecessary detours in areas with dense obstacles and have limited optimization for trajectory smoothness and flight stability. On the other hand, local obstacle avoidance methods, such as artificial potential field methods and dynamic window methods, generate obstacle avoidance strategies by sensing the surrounding environment in real time, exhibiting good response speed. However, they are prone to getting trapped in local optima and struggle to guarantee global target consistency. In practical UAV missions, these methods often require trade-offs between real-time performance, obstacle avoidance capabilities, and trajectory smoothness, making it difficult to achieve a comprehensive balance.

[0004] In summary, existing methods still have shortcomings in comprehensively considering real-time performance, trajectory smoothness, obstacle avoidance capabilities, and dynamic constraints. Especially in complex 3D environments with irregular obstacle distribution, limited UAV perception range, and continuous mission requirements, a trajectory planning strategy that can balance local real-time obstacle avoidance and global target guidance is needed. To address these issues, this invention proposes a UAV obstacle avoidance trajectory planning method with tangent smoothness constraints. By constructing a local 2D navigation plane and extracting obstacle boundary tangents at each planning step to guide waypoint selection, the UAV can quickly respond to local environmental changes while maintaining global target orientation consistency, achieving dynamic obstacle avoidance and trajectory optimization. This method not only improves trajectory smoothness and flight safety but also adapts to complex or dynamically changing 3D obstacles, meeting the needs of practical UAV autonomous flight missions. Summary of the Invention

[0005] This invention provides a UAV obstacle avoidance trajectory planning method with tangent smoothness constraints, aiming to solve the challenge of UAV trajectory planning in complex 3D environments and achieve smooth, collision-free, and time-efficient flight trajectories. The method constructs a navigation plane within the locally visible area of ​​the UAV and generates waypoints using obstacle boundary tangents, thereby ensuring global target alignment while achieving real-time response to dynamic environments.

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

[0007] In practice, the UAV first acquires its current and target positions, calculates the unit direction vector from the current position to the target, and constructs a locally visible region along this direction. Within this visible region, the UAV performs obstacle detection on a 3D discrete mesh environment model to determine if a collision exists along the target direction. If no obstacle is detected, the UAV generates the next candidate waypoint along the target direction with an adaptive step size; if an obstacle is detected, the first collision point is recorded, and a 2D navigation plane orthogonal to the flight direction is constructed at that point. This navigation plane maps the obstacle occupancy status at that location, forming a 2D cross-section, providing a geometric basis for subsequent tangent generation.

[0008] S1. Problem Modeling: Construct a three-dimensional discrete spatial environment model, representing the environment as a set of occupied states composed of several uniform grids, where each grid cell can be marked as free or occupied by obstacles; obtain the current position and final target position of the UAV, and consider the characteristics of obstacles that may change over time in a dynamic environment; construct a locally visible area extending along the current flight direction to enable the UAV to navigate safely under limited perception conditions; establish a UAV kinematic constraint model, including maximum yaw angle constraints and maximum pitch angle constraints, to ensure that the generated waypoints meet the actual flight feasibility.

[0009] S2. Algorithm Design: The algorithm employs an incremental planning strategy, combining local navigation plane construction, tangent guidance, and waypoint sampling to achieve real-time trajectory planning in complex 3D dynamic environments. In each planning step, the UAV generates candidate waypoints based on local perception information and selects the optimal point through a comprehensive evaluation function, while maintaining the consistency of the global target direction. This method balances trajectory smoothness, obstacle avoidance capability, and kinematic feasibility.

[0010] S2.1 Obtain the current position and target position of the UAV, calculate the unit direction vector from the current position to the target position; construct a locally visible region extending along the direction; perform obstacle detection within the visible region based on a three-dimensional discrete mesh environment model;

[0011] S2.2 Collision Detection: Determine whether there is a collision along the target direction. If no obstacle is detected, generate the next candidate position along the target direction according to the adaptive step size to ensure smooth and continuous trajectory generation. If an obstacle is detected, locate the first collision grid cell along the target direction and record its three-dimensional spatial coordinates as the first collision point. At the same time, mark the potential danger area adjacent to the point to provide accurate reference for subsequent local navigation plane and tangent generation.

[0012] S2.3 Local navigation plane construction: Construct a local two-dimensional navigation plane at the initial collision point; map the obstacle occupancy state at the collision section in three-dimensional space to the navigation plane to generate a two-dimensional obstacle section model;

[0013] S2.4, Tangent-based obstacle avoidance direction generation strategy: Identify obstacle boundaries in the two-dimensional navigation plane, including the contour information of continuously occupied grids; start from the first collision point and perform free grid detection along several preset search directions (such as up and down, left and right) to extract multiple boundary tangent points; then connect the current position of the UAV with each tangent point through vectors to generate candidate tangent directions. Each direction vector can be used as a potential obstacle avoidance path, and the deviation angle from the global target direction is recorded to provide a basis for the comprehensive cost assessment of waypoints.

[0014] S2.5 Adaptive waypoint generation based on constraint sampling: Normalize each candidate tangent direction; perform distance sampling along the unit direction according to an adaptive adjustment factor; generate a set of candidate waypoints that satisfy kinematic constraints, including maximum yaw angle limit and maximum pitch angle limit;

[0015] S2.6. Trajectory update mechanism based on smoothness and feasibility assessment: Construct a comprehensive evaluation function that includes angle deviation cost and collision risk cost; calculate the comprehensive cost for each waypoint in the candidate waypoint set; select the waypoint with the minimum comprehensive cost as the optimal waypoint for the current planning step, and update the trajectory sequence.

[0016] S2.7 Repeat steps S2.2 to S2.6 until the target location is reached;

[0017] S3, Trajectory Output: Obtain a collision-free three-dimensional flight trajectory consisting of all optimal waypoints;

[0018] As a preferred embodiment of the present invention, the specific content of the S1 problem modeling is as follows:

[0019] Unmanned aerial vehicle (UAV) trajectory planning aims to find a smooth, collision-free, and time-efficient trajectory in complex 3D environments. In these environments, obstacles may be irregularly shaped and dynamically change over time, and the UAV can only acquire environmental information based on a localized perception area. The trajectory planning problem modeling specifically includes the following:

[0020] (1) Environment and trajectory representation

[0021] The environment is modeled as a discretized three-dimensional space. It is composed of uniform grid cells;

[0022] Each grid cell The occupied state is determined by the function express:

[0023] ;

[0024] The drone trajectory is represented as a sequence of path points. ,starting point ,end ;

[0025] A feasible trajectory must satisfy:

[0026] ;

[0027] In a dynamic environment, the obstacle occupancy state changes over time as follows: At this point, the feasibility of the trajectory requires that each path point has no collision at the corresponding time step.

[0028] (2) Locally visible region constraints

[0029] At each time step The drone senses a fixed-size cuboid region extending from its current position along its current orientation. :

[0030] ;

[0031] in, These represent the length, width, and height of the visible area, respectively.

[0032] (3) Kinematic constraints

[0033] Any adjacent flight segment and Horizontal yaw angle and vertical pitch angle The drone's maneuverability must be met:

[0034] ;

[0035] ;

[0036] in and These represent the maximum permissible yaw angle and pitch angle, respectively.

[0037] (4) Trajectory optimization objective

[0038] trajectory length :

[0039] ;

[0040] trajectory smoothness :

[0041] ;

[0042] The two indicators constitute the optimization objective: to ensure the feasibility and safety of the trajectory, so that... Minimize to reduce flight time and energy consumption, and make Minimize to improve trajectory smoothness and flight stability.

[0043] As a preferred embodiment of the present invention, the collision detection and local navigation plane construction in S2 specifically includes the following steps:

[0044] S4.1, in each planning step Get the current location of the drone and target location ;

[0045] S4.2 Construct a unit vector along the target direction ;

[0046] S4.3, at the current location Construct a locally visible region It also performs obstacle detection; and each time the locally visible area is updated, the detected obstacle information is recorded to the local map to support the next planning step.

[0047] S4.4 Determine if a collision exists along the target direction;

[0048] S4.5 If no collision is detected, proceed along the target direction with an adaptive step size. Generate candidate waypoints ;

[0049] S4.6 If a collision is detected, record the first collision point. And at that location, construct a two-dimensional navigation plane orthogonal to the flight direction. First collision point The selection takes into account the location where the collision first occurs along the target direction, and records the boundary features of the obstacle to provide a geometric basis for tangent generation.

[0050] S4.7 Map the state of obstacles in the navigation plane to a two-dimensional cross section for subsequent tangent generation;

[0051] S4.8. Pass the updated waypoint set to the tangent guidance module for processing.

[0052] As a preferred embodiment of the present invention, the obstacle avoidance direction generation strategy based on tangent construction in S2 specifically includes the following steps:

[0053] S5.1, in the two-dimensional navigation plane Identify obstacle boundaries;

[0054] S5.2, From the point of first collision Start by probing the free grid along several preset directions and extract the set of boundary tangent points. ;

[0055] S5.3, Set the current position Connecting each tangent point to form candidate tangent directions ;

[0056] S5.4 Adaptively sample candidate waypoints along each tangent direction to generate waypoints that satisfy kinematic constraints. The sampling step size can be dynamically adjusted according to the tangent length and obstacle gaps to ensure that candidate waypoints both densely cover possible paths and meet the dynamic constraints of UAVs.

[0057] S5.4.1, Waypoints must meet the maximum yaw angle requirement. limit;

[0058] S5.4.2, Waypoints must meet the maximum pitch angle requirement. limit.

[0059] As a preferred embodiment of the present invention, the adaptive waypoint generation based on constraint sampling in step S2 specifically includes the following steps:

[0060] S6.1, For each tangent line Set adaptive adjustment factor Adaptive adjustment factor The system calculates the distance between the tangent direction and the current position, the drone's speed, and the gaps between obstacles to ensure that all generated candidate waypoints can be actually executed by the drone.

[0061] S6.2. Generate candidate waypoints along the corresponding tangent direction. After determining the valid tangent direction, use the direction vector formed by the current position and the corresponding tangent point as the guiding direction, normalize it, and advance a certain distance along this direction to generate candidate waypoints:

[0062] ;

[0063] Among them, the propulsion distance The waypoints are adaptively determined based on the local visible area and forward collision detection results, so that the generated waypoints are located within the flyable space and satisfy the kinematic constraints of the UAV.

[0064] S6.3 Check whether each candidate waypoint meets the maximum yaw angle of the UAV. and maximum pitch angle Isokinetic constraints;

[0065] S6.4 Use the set of all candidate waypoints that meet the conditions for subsequent evaluation of trajectory smoothness and obstacle avoidance cost function.

[0066] As a preferred embodiment of the present invention, the trajectory update mechanism based on smoothness and feasibility assessment in S2 specifically includes the following steps:

[0067] S7.1 Calculate each candidate waypoint Angular deviation cost ,

[0068] ;

[0069] The angle deviation cost is used to reflect the degree of deviation of the candidate waypoint from the target direction. The smaller the angle, the smoother the trajectory transition and the stronger the target guidance. The larger the angle, the more obvious the course deviation, and the greater the cost in the subsequent comprehensive evaluation.

[0070] S7.2 Calculate the collision risk cost for each candidate waypoint. ,

[0071] ;

[0072] The above calculations can numerically represent the differences in safety among different candidate waypoints, providing a basis for subsequent comprehensive cost calculations and optimal waypoint selection.

[0073] S7.3, According to weight and Calculate the overall cost Weighting of angular deviation cost and collision risk cost and It can be dynamically adjusted according to task priority to balance trajectory smoothness and safety. In complex environments, historical trajectory references can be added to improve planning continuity.

[0074] S7.4 Select the waypoint with the lowest overall cost as the optimal next position. .

[0075] As a preferred embodiment of the present invention, the trajectory output method based on smoothness and feasibility assessment in S2 specifically includes the following steps:

[0076] S8.1 Add the selected waypoints to the trajectory sequence;

[0077] S8.2 Update the current location to the selected waypoint;

[0078] S8.3 Repeat steps S4.1 to S7.4 until the target location is reached;

[0079] S8.4 Output the final trajectory sequence ;

[0080] Compared with the prior art, the advantages of the present invention are as follows:

[0081] 1. An efficient trajectory generation strategy combining local navigation plane and tangent guidance: Most existing 3D trajectory planning methods rely on global search or complex path search, which are computationally intensive and slow in environments with dense and dynamically changing obstacles. This invention constructs a local 2D navigation plane at the initial collision point and extracts tangents from obstacle boundaries to generate candidate waypoints, achieving rapid obstacle avoidance under local perception. Through tangent direction prior and adaptive sampling, the UAV can generate multiple feasible paths within a limited visible area, while ensuring that waypoints satisfy kinematic constraints, thereby significantly improving the real-time performance, executability, and flight safety of trajectory planning.

[0082] 2. A Dynamic Evaluation Mechanism Balancing Smoothness and Safety: Existing trajectory optimization methods often focus on single metrics, such as path length or smoothness, making it difficult to balance safety and smoothness. This invention constructs a comprehensive evaluation function that includes angle deviation costs and collision risk costs. It performs multi-dimensional evaluations on each candidate waypoint and selects the waypoint with the lowest comprehensive cost as the next position. This mechanism ensures global goal consistency while achieving a balance between smooth trajectory transitions and safe obstacle avoidance, preventing traditional methods from producing jump trajectories or local collisions in complex environments.

[0083] 3. Progressive Iterative Generation and Highly Adaptive Planning: Existing methods typically generate the complete path in one step, making it difficult to cope with changes in dynamic environments. This invention uses an iterative approach, selecting the optimal waypoints and updating the trajectory sequence at each planning step. This allows the UAV to adjust its route in a timely manner according to changes in environmental information while maintaining global target orientation. Combining adaptive waypoint sampling and kinematic constraints, this invention can achieve efficient, smooth, and safe real-time trajectory planning in complex 3D environments, and is applicable to various types of unmanned aerial vehicles (UAVs) or underwater vehicles. Attached Figure Description

[0084] Figure 1 The flowchart of the proposed tangent-guided UAV trajectory planning algorithm is shown below.

[0085] Figure 2 A schematic diagram of the distribution of tangent points on the navigation plane.

[0086] Figure 3 Draw a tangent diagram for the navigation plane. Detailed Implementation

[0087] 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 embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0088] Please see Figure 1 This invention provides an implementation method, using trajectory planning of a UAV in a complex three-dimensional elevation terrain environment as an example, to illustrate the proposed three-dimensional trajectory planning method. This method is applicable to flight scenarios with undulating terrain, discrete obstacles, and unknown environmental interference, and can generate smooth, continuous flight trajectories that satisfy dynamic constraints while ensuring flight safety.

[0089] In this embodiment, a three-dimensional elevation terrain model is first constructed. The flight space is discretized into a three-dimensional grid structure, with each grid cell representing the occupancy status and altitude information of the corresponding spatial region. The UAV's starting point and target point are calibrated within this three-dimensional environment model, while the current flight position variable is also set. It is used to update the spatial coordinates of the drone in real time during the planning process.

[0090] After system initialization, a locally visible region is constructed at the current UAV location, and a target guidance trajectory is generated within this region. The target guidance trajectory extends along the direction from the current location towards the target point, and forward collision detection is performed in this direction. When no obstacle is detected within the visible region, the system directly generates the next waypoint. The drone flies to the waypoint and updates its current position; then it determines whether it has reached the target point. If it has not reached the target point, it continues to iterate until the final trajectory is generated and output.

[0091] When a collision risk is detected, the system executes an obstacle avoidance algorithm planning mechanism, including 3D-to-2D navigation plane extraction, tangent-based navigation strategy, adaptive waypoint sampling method, and evaluation and waypoint selection process. First, a navigation plane perpendicular to the current flight direction is constructed at the initial collision point, mapping the 3D obstacle structure to the 2D plane for analysis. Then, obstacle boundary contours are extracted within this plane, and multiple boundary tangent points are calculated. Next, the UAV's current position is connected to the tangent points to form candidate tangent directions. Adaptive sampling is performed along the tangent directions to generate multiple candidate waypoints. Finally, the optimal waypoint is selected through a comprehensive evaluation function, and the flight status is updated. This process is repeated until the UAV reaches the target position, ultimately forming a complete UAV trajectory.

[0092] Specifically, when using it:

[0093] 1. Environment Model Initialization and Flight State Parameter Setting: When implementing the UAV 3D trajectory planning method of this invention, it is first necessary to model the 3D flight environment in which the UAV operates so that the planner can generate a smooth, collision-free trajectory in an environment with complex elevation terrain and irregular obstacle distribution. Specifically, the flight space is discretized into a 3D mesh structure. It is composed of uniform voxel units. Each voxel unit For a discrete location in space, its occupancy state is determined by a function. express:

[0094]

[0095] The drone trajectory is represented as a sequence of path points. The starting point is The destination is Each waypoint Must be located in flyable space Inside. In a dynamic environment, the state of obstacles changes over time as follows: The feasibility of the trajectory requires that each path point does not collide with an obstacle within the corresponding time step.

[0096] During the trajectory planning initialization phase, the system first calibrates the starting position of the UAV. Target location and current flight position The planner then calculates the direction vector from the current position to the target point and standardizes it to obtain the unit direction vector. This direction vector serves as a reference for generating the subsequent target-oriented path. The UAV's flight state parameters, including its current position, velocity, acceleration, and heading attitude, are also initialized at this stage to ensure dynamic consistency during trajectory planning.

[0097] Meanwhile, to ensure that the UAV plans solely based on its own perception capabilities at each time step, this invention defines a locally visible region. This is a rectangular region centered on the current location and extending along the current orientation, used to store currently observable spatial information. This region is defined as follows:

[0098]

[0099] in, These represent the length, width, and height of the locally visible region in the X, Y, and Z directions, respectively. In each planning step, the UAV uses only environmental information within the locally visible region for waypoint generation and collision detection, thus avoiding reliance on global environmental information and making the system adaptable to unknown or dynamically changing environments.

[0100] Furthermore, to ensure the dynamic feasibility of the generated trajectory, the planner also considers kinematic constraints during the initialization phase. Specifically, for any adjacent flight segments... and Horizontal yaw angle and vertical pitch angle The limitations on the maneuverability of drones must be met:

[0101]

[0102]

[0103] in, and These represent the maximum permissible horizontal yaw angle and maximum vertical pitch angle for the UAV, respectively. Through these constraints, the trajectory planning generates a path that is not only safe and reliable but also meets the dynamic constraints of actual UAV flight.

[0104] Through the above steps, the UAV completes environmental modeling and flight state initialization, providing the foundation for subsequent local navigation plane construction, tangent guidance, and waypoint generation. After initialization, the system can proceed to the next stage of trajectory generation, namely, forward collision detection and waypoint planning based on the locally visible area.

[0105] 2. Local navigation plane construction and collision detection

[0106] After completing the environmental model initialization and flight state parameter setting, the UAV trajectory planning method enters the local navigation phase. In this phase, the system determines the path based on the current flight position. A local navigation plane is constructed to enable timely obstacle detection and avoidance, thereby ensuring the feasibility and safety of the trajectory. This process specifically includes the following steps.

[0107] First, the planner uses the drone's current location. For reference, determine the target point. The guiding direction. This direction is established by connecting the current position and the target position, forming the reference direction for the planner's forward search. Within the locally visible area... Within this system, the flight space is progressively scanned along the guide direction to detect potential collisions. During the scan, the planner determines the occupancy status along the guide line cell by cell, one by one. Once an occupied unit is detected, it is identified as an obstacle, and that point is defined as the first collision point. .

[0108] At the point of first collision, the planner constructs a two-dimensional navigation plane perpendicular to the current flight direction. This plane is typically... Using the axis as the normal, the 3D environment information is mapped to a 2D projection. In this way, the 3D obstacle structure is transformed into a 2D contour on the local plane, making subsequent obstacle avoidance path calculation and tangent generation simpler and more efficient. On the navigation plane, the system extracts all feasible spatial grids connected to the initial collision point, removes areas occupied by obstacles, and retains only the grid cells of the outer boundary as the obstacle boundary point set. This boundary point set provides the basic geometric information for subsequent tangent generation and waypoint selection.

[0109] If no obstacle is detected along the guiding direction in the locally visible area, the planner will directly proceed along the unit direction vector. Generate the next waypoint The drone will then fly to that point and update its current location. The system then determines whether the drone has reached the target location. If the destination is not reached, the forward search and waypoint generation continue iteratively until a complete trajectory is formed.

[0110] In actual flight, obstacles may have irregular shapes and be densely distributed, requiring the planner to flexibly adapt to different local environments. Therefore, the navigation plane is not only used for a two-dimensional representation of obstacles but also serves as a local obstacle avoidance reference for the UAV at the current time step. The boundary points extracted by the planner in the plane can effectively characterize the obstacle contours, preventing unnecessary backtracking or detours by the UAV in complex environments, thus ensuring the smoothness and continuity of the trajectory.

[0111] Furthermore, to enhance the planner's real-time responsiveness, each local navigation plane is dynamically generated at the UAV's current location and reconstructed after each planning step based on the new location. This incremental update mechanism ensures that the UAV can quickly adjust its trajectory and avoid collisions even when new obstacles or dynamic changes occur in the environment. The overall process of navigation plane construction and collision detection is as follows: The UAV first scans the forward mesh in the locally visible area to identify the first collision point; then, it constructs a vertical navigation plane at that point, projecting the 3D obstacle into a 2D contour; next, it extracts a set of boundary points from the contour to provide the geometric basis for tangent guidance and waypoint generation; finally, it determines whether to proceed directly along the target direction or perform obstacle avoidance path planning based on local environmental information.

[0112] By employing this local navigation plane construction and collision detection method, UAVs can achieve timely obstacle perception and precise obstacle avoidance in complex 3D environments. This step not only ensures flight safety but also provides the necessary conditions for subsequent tangent-based obstacle avoidance direction generation and adaptive waypoint sampling, enabling the entire trajectory planning process to balance target orientation and local safety.

[0113] 3. Tangent-based obstacle avoidance direction generation strategy:

[0114] In this embodiment, once the UAV detects an obstacle in a locally visible area, the system enters the obstacle avoidance direction generation phase based on tangent construction. In this phase, a local navigation plane perpendicular to the flight direction is first constructed at the initial collision point, such as... Figure 2 As shown, on this plane, the three-dimensional structure of the obstacle is projected as a two-dimensional cross-section, thus forming a clear boundary outline. The construction of the navigation plane enables UAVs to simplify the complex three-dimensional obstacle avoidance problem into a two-dimensional analysis problem, reducing the amount of computation while ensuring the feasibility and response speed of the obstacle avoidance strategy.

[0115] Within the constructed navigation plane, the system identifies passable boundary tangents by scanning obstacle boundaries. Each tangent is located outside the obstacle's outline, representing a potential passage around the obstacle. A tangent line is formed between the UAV's current position and each tangent, and each tangent line corresponds to a possible detour direction. For example... Figure 3 As shown, these tangents exhibit different directions and lengths on the plane, covering the feasible area around the obstacle. In this way, the system can quickly obtain multiple candidate headings without having to perform time-consuming path searches throughout space.

[0116] The described tangent construction strategy is dynamically adaptive. During the UAV's movement, the local navigation plane and tangent set dynamically change with position updates, and the selection of tangent points is adjusted based on real-time environmental information. When a new obstacle enters the visible area or the position of an existing obstacle changes, the tangent points and tangents are updated instantly, ensuring that the UAV can always obtain a feasible obstacle avoidance direction in complex or dynamic environments. Tangents not only provide directional priors for waypoint generation but also guarantee the continuity and directional stability of the UAV's trajectory, avoiding significant backtracking or trajectory oscillations.

[0117] Furthermore, the tangent construction strategy balances target orientation and safety. When generating tangents, the system prioritizes those with a small angle to the target direction, thus maintaining the original flight path as much as possible while avoiding obstacles. This method allows the UAV to quickly circumvent obstacles while maintaining a smooth overall trajectory toward the target, achieving a smooth and continuous flight path. Simultaneously, by generating and comparing multiple tangents in parallel, the UAV can select the optimal circumvention direction in complex obstacle environments, improving obstacle avoidance success rate and flight efficiency.

[0118] In summary, this implementation provides a reliable and efficient local obstacle avoidance direction for UAVs by extracting obstacle boundary tangent points and generating tangent lines on the local navigation plane. This method has the advantages of clear structure, low computational cost, and strong real-time performance, enabling continuous, safe, and smooth trajectory planning in complex 3D environments, providing a solid foundation for subsequent candidate waypoint generation and trajectory updates.

[0119] 4. An Adaptive Waypoint Generation Method Based on Constraint Sampling

[0120] After completing tangent construction and obstacle avoidance direction generation, this implementation enters the adaptive waypoint generation stage based on constraint sampling. In this stage, the system generates waypoints along each candidate tangent to guide the UAV's specific movement in the local environment. Each tangent represents a feasible obstacle avoidance direction, while the waypoint is the actual spatial position of the UAV moving along the tangent, ensuring the executability and dynamic feasibility of the trajectory.

[0121] Specifically, when sampling waypoints along the tangential direction from the UAV's current position, the system first determines the sampling position on each tangent based on the current flight status and obstacle distribution. The sampling interval and distance are dynamically adjusted according to the UAV's mobility, ensuring that each waypoint satisfies the UAV's maneuverability constraints while allowing it to smoothly traverse local obstacle areas. Each sampling point is generated with consistency to the current heading in mind, ensuring a smooth trajectory for the UAV moving along the tangent, avoiding sharp turns or discontinuous movements, thereby improving trajectory smoothness and controllability.

[0122] Furthermore, this implementation also performs a local feasibility check on the waypoints generated by each tangent. The system detects whether candidate waypoints are located outside of obstacle-occupied areas and compares them with other tangent information within the local navigation plane to eliminate unsafe or unreachable points. In this way, the system can respond in real time to dynamically changing environments when generating waypoints, such as newly appearing obstacles or slight shifts in the positions of existing obstacles, thereby ensuring that the UAV always has a safe and feasible direction of travel in complex three-dimensional space.

[0123] After completing adaptive waypoint sampling, this implementation method comprehensively evaluates candidate waypoints through a trajectory update mechanism to select the next flight target point. The design goal of this mechanism is to balance trajectory smoothness and obstacle avoidance safety, enabling the UAV to fly continuously, stably, and safely to the target location in complex three-dimensional environments.

[0124] Specifically, for each candidate waypoint generated by tangent sampling The system calculates the angular deviation of the drone relative to the target direction and its distance from surrounding obstacles to obtain a comprehensive evaluation index. Point to candidate waypoints Direction and target direction The degree of deviation between them. A smaller angle indicates a smoother heading change and a smoother trajectory transition, thus reducing drastic changes in direction during UAV maneuvers. This can be formally represented as:

[0125]

[0126] At the same time, the system assesses the collision risk of candidate waypoints. Specifically, this involves calculating the collision risk of candidate waypoints. Shortest distance to the nearest obstacle This distance is then converted into a risk value. The closer the distance, the greater the risk; the farther the distance, the lower the risk, thus ensuring that the drone stays as far away from obstacles as possible when planning its flight path, guaranteeing flight safety. The risk assessment formula is as follows:

[0127]

[0128] After obtaining smoothness and risk assessment indicators, the system sets weights. and The combined weighted average of the two factors yields the comprehensive value of the candidate waypoints. :

[0129]

[0130] In this embodiment, the weights are typically set to This means achieving a balance between trajectory smoothness and obstacle avoidance safety. The weights can also be adjusted appropriately based on flight environment density and mission requirements, for example, increasing them in areas with dense obstacles. The value is increased to enhance security; in open areas, the value is improved. The value of is adjusted to optimize trajectory smoothness.

[0131] Subsequently, the system selects the waypoint with the lowest overall cost from the candidate waypoint set as the next location. :

[0132]

[0133] The selected waypoints will be used to update the drone's current position and serve as the starting point for the next planning step. By iteratively executing this process, the drone can gradually generate a continuous trajectory within a locally visible area, while responding in real time to new obstacles appearing in the environment and avoiding collisions. The entire process is repeated during flight until the drone reaches the target location. This results in a smooth, safe, and flight-dynamically compliant trajectory.

Claims

1. A method for planning obstacle avoidance trajectories for unmanned aerial vehicles (UAVs) with tangent smoothness constraints, characterized in that, Specifically, the following steps are included: S1. Problem Modeling: Construct a three-dimensional discrete spatial environment model, representing the environment as a set of occupied states composed of several uniform grids, where each grid cell can be marked as free or occupied by obstacles; obtain the current position and final target position of the UAV, and consider the characteristics of obstacles that may change over time in a dynamic environment; construct a locally visible area extending along the current flight direction to enable the UAV to navigate safely under limited perception conditions; establish a UAV kinematic constraint model, including maximum yaw angle constraints and maximum pitch angle constraints, to ensure that the generated waypoints meet the actual flight feasibility requirements; S2. Algorithm Design: The algorithm employs an incremental planning strategy, combining local navigation plane construction, tangent guidance, and waypoint sampling to achieve real-time trajectory planning in complex 3D dynamic environments. In each planning step, the UAV generates candidate waypoints based on local perception information and selects the optimal point through a comprehensive evaluation function, while maintaining the consistency of the global target direction. This method balances trajectory smoothness, obstacle avoidance capability, and kinematic feasibility. S2.1 Obtain the current position and target position of the UAV, calculate the unit direction vector from the current position to the target position; construct a locally visible region extending along the direction; perform obstacle detection within the visible region based on a three-dimensional discrete mesh environment model; S2.2 Collision Detection: Determine whether there is a collision along the target direction. If no obstacle is detected, generate the next candidate position along the target direction according to the adaptive step size to ensure smooth and continuous trajectory generation. If an obstacle is detected, locate the first collision grid cell along the target direction and record its three-dimensional spatial coordinates as the first collision point. At the same time, mark the potential danger area adjacent to the point to provide accurate reference for subsequent local navigation plane and tangent generation. S2.3 Local navigation plane construction: Construct a local two-dimensional navigation plane at the initial collision point; map the obstacle occupancy state at the collision section in three-dimensional space to the navigation plane to generate a two-dimensional obstacle section model; S2.4, Tangent-based obstacle avoidance direction generation strategy: Identify obstacle boundaries in the two-dimensional navigation plane, including the contour information of continuously occupied grids; Starting from the initial collision point, free grid detection is performed along several preset search directions (such as up and down, left and right) to extract multiple boundary tangent points; then, the current position of the UAV and each tangent point are connected by vectors to generate candidate tangent directions. Each direction vector can be used as a potential obstacle bypass path. At the same time, the deviation angle from the global target direction is recorded to provide a basis for the comprehensive cost assessment of waypoints. S2.5, Adaptive waypoint generation based on constraint sampling: Normalize each candidate tangent direction; Distance sampling is performed along the unit direction according to an adaptive adjustment factor; a set of candidate waypoints satisfying kinematic constraints is generated, the kinematic constraints including maximum yaw angle limit and maximum pitch angle limit; S2.

6. Trajectory update mechanism based on smoothness and feasibility assessment: Construct a comprehensive evaluation function that includes angle deviation cost and collision risk cost; calculate the comprehensive cost for each waypoint in the candidate waypoint set; select the waypoint with the minimum comprehensive cost as the optimal waypoint for the current planning step, and update the trajectory sequence. S2.7 Repeat steps S2.2 to S2.6 until the target location is reached; S3, Trajectory Output: Obtain a collision-free three-dimensional flight trajectory consisting of all optimal waypoints.

2. The UAV obstacle avoidance trajectory planning method with tangent smoothness constraint according to claim 1, characterized in that: The specific details of the S1 problem modeling are as follows: Unmanned aerial vehicle (UAV) trajectory planning aims to find a smooth, collision-free, and time-efficient trajectory in complex 3D environments. In these environments, obstacles may be irregularly shaped and dynamically change over time, and the UAV can only acquire environmental information based on a localized perception area. The trajectory planning problem modeling specifically includes the following: (1) Environment and trajectory representation The environment is modeled as a discretized three-dimensional space. It is composed of uniform grid cells; Each grid cell The occupancy state is determined by the function express: ； The drone trajectory is represented as a sequence of path points. ,starting point ,end ; A feasible trajectory must satisfy: ； In a dynamic environment, the obstacle occupancy state changes over time as follows: At this point, the feasibility of the trajectory requires that each path point has no collision at the corresponding time step; (2) Locally visible region constraints At each time step The drone senses a fixed-size cuboid region extending from its current position along its current orientation. : ； in, These represent the length, width, and height of the visible area, respectively. (3) Kinematic constraints Any adjacent flight segments and Horizontal yaw angle and vertical pitch angle The drone must meet the following maneuverability requirements: ； ； in and These represent the maximum permissible yaw and pitch angles, respectively. (4) Trajectory optimization objective trajectory length : ； trajectory smoothness : ； The two indicators constitute the optimization objective: to ensure the feasibility and safety of the trajectory, so that... Minimize to reduce flight time and energy consumption, and make Minimize to improve trajectory smoothness and flight stability.

3. The UAV obstacle avoidance trajectory planning method with tangent smoothness constraint according to claim 1, characterized in that: The collision detection and local navigation plane construction in S2 specifically includes the following steps: S4.1, in each planning step Get the current location of the drone and target location ; S4.2 Construct a unit vector along the target direction ; S4.3, at the current location Construct a locally visible region It also performs obstacle detection; and each time the locally visible area is updated, the detected obstacle information is recorded to the local map to support the next planning step. S4.4 Determine if a collision exists along the target direction; S4.5 If no collision is detected, proceed along the target direction with an adaptive step size. Generate candidate waypoints ; S4.6 If a collision is detected, record the first collision point. And at that location, construct a two-dimensional navigation plane orthogonal to the flight direction. First collision point The selection takes into account the location where the collision first occurs along the target direction, and records the boundary features of the obstacle to provide a geometric basis for tangent generation. S4.7 Map the state of obstacles in the navigation plane to a two-dimensional cross section for subsequent tangent generation; S4.

8. Pass the updated waypoint set to the tangent guidance module for processing.

4. The UAV obstacle avoidance trajectory planning method with tangent smoothness constraint according to claim 1, characterized in that: The specific steps of the obstacle avoidance direction generation strategy based on tangent construction are as follows: S5.1, in the two-dimensional navigation plane Identify obstacle boundaries; S5.2, From the point of first collision Start by probing the free grid along several preset directions and extract the set of boundary tangent points. ; S5.3, Set the current position Connecting each tangent point to form candidate tangent directions ; S5.4 Adaptively sample candidate waypoints along each tangent direction to generate waypoints that satisfy kinematic constraints. ; The sampling step size can be dynamically adjusted according to the tangent length and obstacle gaps to ensure that candidate waypoints both densely cover possible paths and meet UAV dynamic constraints. S5.4.1, Waypoints must meet the maximum yaw angle requirement. limit; S5.4.2, Waypoints must meet the maximum pitch angle requirement. limit.

5. The UAV obstacle avoidance trajectory planning method with tangent smoothness constraint according to claim 1, characterized in that: The adaptive waypoint generation based on constraint sampling in S2 is as follows: S6.1, For each tangent line Set adaptive adjustment factor ; Adaptive adjustment factor Based on the distance between the tangent direction and the current position, the drone speed, and the clearance between obstacles, calculations are performed to ensure that all generated candidate waypoints can be actually executed by the drone. S6.

2. Generate candidate waypoints along the corresponding tangent direction. After determining the valid tangent direction, use the direction vector formed by the current position and the corresponding tangent point as the guiding direction, normalize it, and advance a certain distance along this direction to generate candidate waypoints: ； Among them, the propulsion distance The waypoints are adaptively determined based on the local visible area and the forward collision detection results, so that the generated waypoints are located within the flyable space and satisfy the kinematic constraints of the UAV. S6.3 Check whether each candidate waypoint meets the maximum yaw angle of the UAV. and maximum pitch angle Isokinetic constraints; S6.4 Use the set of all candidate waypoints that meet the conditions for subsequent evaluation of trajectory smoothness and obstacle avoidance cost function.

6. The UAV obstacle avoidance trajectory planning method with tangent smoothness constraint according to claim 1, characterized in that: The trajectory update mechanism based on smoothness and feasibility assessment in S2 is as follows: S7.1 Calculate each candidate waypoint Angular deviation cost , ； The angle deviation cost is used to reflect the degree of deviation of the candidate waypoint from the target direction. The smaller the angle, the smoother the trajectory transition and the stronger the target guidance. The larger the angle, the more obvious the course deviation, and the greater the cost in the subsequent comprehensive evaluation. S7.2 Calculate the collision risk cost for each candidate waypoint. , ； The above calculations can numerically represent the differences in safety among different candidate waypoints, providing a basis for subsequent comprehensive cost calculations and optimal waypoint selection. S7.3, According to weight and Calculate the overall cost ; Weighting of angular deviation cost and collision risk cost and It can be dynamically adjusted according to task priority to balance trajectory smoothness and safety. In complex environments, historical trajectory references can be added to improve planning continuity; S7.4 Select the waypoint with the lowest overall cost as the optimal next position. .

7. The UAV obstacle avoidance trajectory planning method with tangent smoothness constraint according to claim 1, characterized in that: The trajectory output method in S2 is as follows: S8.1 Add the selected waypoints to the trajectory sequence; S8.2 Update the current location to the selected waypoint; S8.3 Repeat steps S4.1 to S7.4 until the target location is reached; S8.4 Output the final trajectory sequence .

Images