Unmanned aerial vehicle dynamic obstacle avoidance method based on safe flight corridor

Abstract

The invention discloses an unmanned aerial vehicle dynamic obstacle avoidance method based on a safe flight corridor, and the method comprises the following steps: constructing a safe flight corridor model and a potential field constraint model, and planning an initial flight path for an unmanned aerial vehicle; controlling the unmanned aerial vehicle to fly along a planned initial flight path, detecting a dynamic obstacle in real time, if the obstacle is detected to enter a set distance threshold value, calculating an expected acceleration of the unmanned aerial vehicle by using the potential field constraint model, predicting positions of the unmanned aerial vehicle and the obstacle, and then, executing the step 3; verifying whether the predicted position of the unmanned aerial vehicle meets a constraint condition, and if not, correspondingly increasing the weight of the corresponding potential field until the predicted position of the unmanned aerial vehicle meets the constraint condition; and monitoring whether the unmanned aerial vehicle completes obstacle avoidance, and if so, controlling the unmanned aerial vehicle to return to the planned initial flight path and continue to fly. According to the dynamic obstacle avoidance method for the unmanned aerial vehicle, real-time obstacle avoidance of sudden dynamic obstacles in a safe flight corridor can be effectively realized, and the flight safety of the unmanned aerial vehicle is ensured.

Description

Technical Field

[0001] The present invention relates to the field of unmanned aerial vehicle (UAV) trajectory planning, and in particular proposes a dynamic obstacle avoidance method for UAVs based on a safe flight corridor. Background Art

[0002] With the rapid development of drone technology, drones have been widely used in logistics, emergency rescue, environmental monitoring, and inspection. During actual flight, drones often need to autonomously plan paths and avoid obstacles in complex environments, especially in dense and dynamically changing environments such as urban and forested areas. Safe and efficient obstacle avoidance has become a key technology for autonomous drone flight. Existing drone obstacle avoidance methods primarily include static obstacle avoidance methods based on global path planning and dynamic obstacle avoidance methods based on local perception. Global path planning methods often rely on prior environmental information and are difficult to handle when dynamic obstacles suddenly appear during flight. Local obstacle avoidance methods, while able to respond to environmental changes in real time, are prone to falling into local optima in complex environments, even causing the flight path to deviate from safe zones, posing potential safety risks. In recent years, safe flight corridors, as an effective spatial constraint, have been widely used in drone path planning and safe flight management. By presetting a spatial path for drones, safe flight corridors can effectively reduce the complexity of path planning and improve flight safety. However, in the existing technology, there is little research on efficient obstacle avoidance methods for sudden dynamic obstacles in safe flight corridors, and there is still a lack of an efficient solution that can combine safe corridor spatial constraints with dynamic obstacle avoidance methods.

[0003] Therefore, how to achieve efficient and real-time obstacle avoidance of sudden dynamic obstacles within the safe flight corridor and ensure that the drone always flies safely within the safe area has become a technical problem that needs to be solved urgently. Summary of the Invention

[0004] In view of this, the present invention proposes a dynamic obstacle avoidance method for a UAV based on a safe flight corridor to solve the problems existing in the prior art.

[0005] The technical solution provided by the present invention is: a dynamic obstacle avoidance method for a UAV based on a safe flight corridor, comprising the following steps:

[0006] S1: Construct a safe flight corridor model and a potential field constraint model, and plan the initial flight path for the UAV. The potential field constraint model is as follows:

[0007] ;

[0008] Where, represents the integrated potential field, represents the safe flight corridor boundary constraint potential field, represents the repulsive potential field of the dynamic obstacle, represents the attractive potential field at the target position, 、 、 They represent the weights of the safe flight corridor boundary constraint potential field, the dynamic obstacle repulsion potential field, and the target position attraction potential field respectively;

[0009] S2: Control the drone to fly along the planned initial flight path and detect dynamic obstacles in real time. If the distance between the drone and the dynamic obstacle is less than the dynamic obstacle repulsion distance threshold, execute S3;

[0010] S3: Calculate the expected acceleration of the UAV using the potential field constraint model and predict the positions of the UAV and the obstacle. Then, verify whether the predicted position of the UAV satisfies the safe flight corridor boundary constraint conditions and the safe distance constraint conditions from the obstacle. If not, increase the weight of the safe flight corridor boundary constraint potential field and / or the dynamic obstacle repulsion potential field accordingly until the predicted position of the UAV satisfies the constraints and the UAV is controlled to fly under the action of the comprehensive potential field.

[0011] S4: Monitor whether the drone has completed obstacle avoidance. If so, control the drone to return to the planned initial flight path and continue flying.

[0012] Preferably, in S1,

[0013] ;

[0014] Where, , indicating the current position of the drone To the center line of the corridor The shortest distance, is the boundary potential field coefficient, The radius of the corridor representing the safe flight corridor;

[0015] ;

[0016] Where, represents the obstacle repulsion coefficient, Indicates the distance threshold of the dynamic obstacle repulsion force, Indicates the drone and obstacles Dynamic distance;

[0017] ;

[0018] Where, is the attractive potential field coefficient, and Represent the positions of the UAV and the target respectively.

[0019] Further optimization, in S3, the expected acceleration of the drone The calculation formula is as follows:

[0020] ;

[0021] in, .

[0022] Further preferably, in S3, the predicted position of the drone is calculated using the following formula: , where and Represent the predicted position and current position of the drone respectively, Indicates the speed of the drone, represents the desired acceleration of the drone;

[0023] obstacle The predicted position is calculated using the following formula: , where and Represent obstacles The predicted and current locations of Indicates obstacles speed.

[0024] Further preferably, in S3, if the predicted position of the UAV does not meet the safe flight corridor boundary constraint conditions, the weight of the large safe flight corridor boundary constraint potential field is increased; if the predicted position of the UAV does not meet the safe distance constraint conditions from the obstacle, the weight of the dynamic obstacle repulsion potential field is increased; if the predicted position of the UAV does not meet the safe flight corridor boundary constraint and the safe distance constraint from the obstacle at the same time, the weight of the safe flight corridor boundary constraint potential field and the weight of the dynamic obstacle repulsion potential field are increased at the same time.

[0025] Further preferably, if the predicted position of the UAV does not satisfy both the safe flight corridor boundary constraint and the safe distance constraint from the obstacle, the weight increase of the dynamic obstacle repulsion potential field is greater than the weight increase of the safe flight corridor boundary constraint potential field.

[0026] Further preferably, in S4, the method for controlling the UAV to return to the planned initial flight path is as follows:

[0027] Detect drones and any obstacles Are all satisfied ,in, is a safety margin. If it is satisfied, the potential field weight is readjusted and the UAV is made to fly to the initial flight path under the action of the comprehensive potential field after adjusting the potential field weight. In the comprehensive potential field, the weight of the target position attraction potential field is calculated by the following formula:

[0028] ;

[0029] The weight function of the potential field at the boundary of the safe flight corridor is calculated by the following formula:

[0030] ;

[0031] Where, = { , i=1,2,...,N}, represents the distance from the UAV to the nearest obstacle, , , , .

[0032] Further preferably, in S4, the center line of the safe flight corridor is set as the initial flight path, wherein the method for determining the return to the center line of the safe flight corridor is as follows:

[0033] Monitor in real time whether the distance between the current position of the drone and the center line of the safe flight corridor is close and whether the current speed direction of the drone is close to the expected speed direction. If so, determine that the drone has returned to the center line of the safe flight corridor.

[0034] The dynamic obstacle avoidance method for drones based on safe flight corridors proposed in the present invention can effectively realize real-time obstacle avoidance of sudden dynamic obstacles within the safe flight corridor, ensuring the flight safety of the drone. By combining the potential field method with the boundary constraints of the safe flight corridor, the problem of the drone deviating from the safe area during the obstacle avoidance process is avoided, and the problems of local minimum and insufficient spatial constraints existing in the traditional artificial potential field method are solved. In addition, the parameters in this method are adaptively adjusted to adapt to complex and changing environments, and it has strong robustness and practicality. BRIEF DESCRIPTION OF THE DRAWINGS

[0035] The present invention will be further described in detail below with reference to the accompanying drawings and embodiments:

[0036] Figure 1 A flow chart of the dynamic obstacle avoidance method for a UAV based on a safe flight corridor provided by the present invention;

[0037] Figure 2 A schematic diagram of the trajectory of the UAV performing dynamic obstacle avoidance within the safe flight corridor;

[0038] Figure 3 Schematic diagram of the safety corridor boundary potential field, target gravitational potential field, dynamic obstacle repulsive potential field, and comprehensive potential field. DETAILED DESCRIPTION

[0039] The present invention will be further explained below with reference to specific embodiments, but the present invention is not limited thereto.

[0040] like Figure 1 As shown, the present invention provides a method for dynamic obstacle avoidance of a UAV based on a safe flight corridor, comprising the following steps:

[0041] S1: Construct a safe flight corridor model and potential field constraint model, and plan the initial flight path for the UAV;

[0042] Among them, the corridor radius of the safe flight corridor The design principles are as follows:

[0043] (1) ,in, is the maximum diagonal length of the drone. This design principle ensures that the drone has enough maneuvering space in the corridor;

[0044] (2) Corridor radius The recommended value range is 0.5~3 meters, and the specific value can be adjusted dynamically according to the task and environment.

[0045] The centerline of the safe flight corridor can be automatically generated by combining global path planning algorithms such as A* and RRT* with the environmental map. It supports various forms such as straight lines, curves, and segments, and uses spline interpolation for smooth fitting to obtain a parameterized expression C(s).

[0046] Wherein, the potential field constraint model is as follows:

[0047] ;

[0048] Where, represents the integrated potential field, represents the safe flight corridor boundary constraint potential field, represents the repulsive potential field of the dynamic obstacle, represents the attractive potential field at the target position, 、 、 They represent the weights of the safe flight corridor boundary constraint potential field, the dynamic obstacle repulsion potential field, and the target position attraction potential field respectively;

[0049] in, ;

[0050] Where, , indicating the current position of the drone To the center line of the corridor The shortest distance, is the boundary potential field coefficient, preferably, The value range is 20~100. The bigger the return force, the stronger the The radius of the safe flight corridor, where the current position of the UAV Can be acquired in real time through multi-source fusion of GNSS, IMU, and visual odometry;

[0051] in, ;

[0052] Where, represents the obstacle repulsion coefficient, preferably, The value range is 5~30. The bigger it is, the more sensitive the obstacle avoidance response is. Represents the dynamic obstacle repulsion distance threshold, that is, the minimum safe distance threshold between the drone and the obstacle, preferably, The value range is 1~3 m. >2~3 times the maximum size of the drone, Indicates the drone and obstacles The dynamic distance is calculated as follows:

[0053]

[0054] Where, Indicates that the drone is at time The position at the time, Indicates obstacles The initial position of Indicates obstacles The speed at which the obstacle The position of the vehicle can be detected by the laser radar and camera sensors installed on the drone, and the speed can be measured by the millimeter wave radar;

[0055] in, ;

[0056] Where, is the attractive potential field coefficient, preferably, The value range is 0.5~2, The bigger the attraction, the stronger it is. and Represent the positions of the UAV and the target respectively;

[0057] This step lays the foundation for safe and efficient flight of drones in complex and dynamic environments by establishing clear safe flight boundaries and intelligent constraint mechanisms, ensuring that drones always fly within protected airspace while providing a benchmark trajectory for efficient mission execution.

[0058] S2: Control the drone to fly along the planned initial flight path and detect dynamic obstacles in real time. If the distance between the drone and the dynamic obstacle is less than the dynamic obstacle repulsion distance threshold, When , execute S3;

[0059] When the distance between the drone and the dynamic obstacle is less than the dynamic obstacle repulsion distance threshold When the obstacle is avoided, the efficiency and real-time performance of obstacle avoidance can be improved;

[0060] S3: Calculate the expected acceleration of the UAV using the potential field constraint model The positions of the UAV and the obstacle are predicted. After that, the predicted position of the UAV is verified to see whether it meets the safe flight corridor boundary constraint conditions and the safe distance constraint conditions from the obstacle. If not, the weight of the safe flight corridor boundary constraint potential field and / or the dynamic obstacle repulsion potential field is increased accordingly until the predicted position of the UAV meets the constraint conditions and the UAV is controlled to fly under the action of the comprehensive potential field to achieve obstacle avoidance. The obstacle avoidance process is as follows: Figure 2 As shown;

[0061] Among them, the expected acceleration of the drone The calculation formula is as follows:

[0062] ;

[0063] in, ;

[0064] like Figure 3 As shown in the figure, the potential field is low within the safe flight corridor boundary and high outside the boundary. Under the action of the integrated potential field, the drone will be pushed away from the high potential energy area (obstacles, corridor boundary) and pulled towards the low potential energy area (target point). The gradient calculation can be done using the finite difference method or analytical method.

[0065] Among them, the predicted position of the drone can be calculated by the following formula: , where and Represent the predicted position and current position of the drone respectively, Indicates the speed of the drone, represents the desired acceleration of the drone;

[0066] obstacle The predicted position can be calculated by the following formula: , where and Represent obstacles The predicted and current locations of Indicates obstacles speed;

[0067] Among them, if the predicted position of the UAV does not meet the boundary constraints of the safe flight corridor, that is: > R, then increase the weight of the large safe flight corridor boundary constraint potential field (the increase is between 0.05 and 0.15) to make it return to the safe flight corridor as soon as possible;

[0068] If the predicted position of the drone does not meet the safety distance constraint from the obstacle, that is: < , then increase the weight of the dynamic obstacle repulsive potential field (increase by 0.1~0.3) to keep it away from the obstacle:

[0069] If the predicted position of the UAV does not satisfy both of the above constraints, the weight of the safe flight corridor boundary constraint potential field and the weight of the dynamic obstacle repulsion potential field are increased simultaneously. Preferably, the increase in the weight of the dynamic obstacle repulsion potential field is greater than the increase in the weight of the safe flight corridor boundary constraint potential field.

[0070] This step achieves a balance between safety and maneuverability through obstacle avoidance decisions and adaptive weight adjustment, ensuring that the drone can effectively avoid obstacles while staying within the safety corridor during the obstacle avoidance process, thus establishing a reliable closed-loop control mechanism.

[0071] S4: Monitor whether the drone has completed obstacle avoidance. If so, control the drone to return to the planned initial flight path and continue flying;

[0072] Among them, the method of controlling the initial flight path of the UAV regression planning is as follows:

[0073] Detect drones and any obstacles Are all satisfied ,in, As a safety margin, the value is selected as 0.5 meters, which defines the obstacle avoidance threshold The length of the transition interval between the target position and the completely safe state is calculated. If it is satisfied, it means that the UAV has entered the safe area, and the potential field weight is readjusted so that the UAV can fly to the initial flight path under the action of the comprehensive potential field. In the comprehensive potential field, the weight of the target position attraction potential field is calculated by the following formula:

[0074] ;

[0075] The weight function of the potential field at the boundary of the safe flight corridor is calculated by the following formula:

[0076] ;

[0077] Where, = { , i=1,2,...,N}, represents the distance from the UAV to the nearest obstacle, , , , The above values ​​are determined based on a large number of simulation experiments and flight test results. The setting of the empirical parameter values ​​comprehensively considers flight safety, trajectory smoothness, and mission efficiency. While ensuring the safe obstacle avoidance of the UAV, it can achieve efficient path regression and mission continuity, ensuring that the UAV can smoothly return to the planned route and continue to perform its original mission after completing obstacle avoidance.

[0078] Typically, the initial flight path is set to the centerline of the safe flight corridor;

[0079] The method for determining the centerline of the return safe flight corridor is as follows:

[0080] As the drone gradually approaches the centerline of the safe flight corridor under the guidance of the dynamically adjusted potential field weights, it monitors in real time the distance between its current position and the centerline of the safe flight corridor and whether its current speed direction is close to the expected speed direction. When the following conditions are met, it is determined that the return to the centerline is complete and the flight is carried out according to the previously generated planned path:

[0081] ;

[0082] Where, is the position threshold, and the preferred value is 0.3 meters. is the speed direction threshold, and the preferred value is 0.1. , where The point closest to the center line of the safe flight corridor The tangent vector at is the expected flight speed.

[0083] Among them, the optimal parameter s is gradually approached through iterative calculation to obtain a convergent solution and ensure that the true closest point is found , using gradient descent method to solve :

[0084] ;

[0085] in, is the learning rate, which is 0.1, and the iteration termination condition is , When the iteration converges, the final That is what you want .

[0086] The above method was tested through simulation. It was found that the drones could detect obstacles in real time and dynamically adjust their flight trajectory (with a smooth flight trajectory) without deviating from the safe corridor, successfully avoiding obstacles and ultimately reaching the target point.

[0087] Those skilled in the art will appreciate that the embodiments of the present application may be provided as methods, systems, or computer program products. Therefore, the present application may take the form of a fully hardware embodiment, a fully software embodiment, or an embodiment combining software and hardware. Furthermore, the present application may take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code. The solutions in the embodiments of the present application may be implemented using various computer languages, such as the object-oriented programming language Java and the interpreted scripting language JavaScript.

[0088] The present application is described with reference to the flowcharts and / or block diagrams of the methods, devices (systems), and computer program products according to the embodiments of the present application. It should be understood that each process and / or block in the flowchart and / or block diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing device to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing device generate instructions for implementing the processes in the flowchart and / or block diagram. Figure 1 a process or multiple processes and / or boxes Figure 1 A device that provides the functions specified in a block or multiple blocks.

[0089] These computer program instructions may also be stored in a computer readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer readable memory produce an article of manufacture comprising an instruction device, which implements the process Figure 1 a process or multiple processes and / or boxes Figure 1 The function specified in one or more boxes.

[0090] These computer program instructions can also be loaded onto a computer or other programmable data processing device so that a series of operational steps are executed on the computer or other programmable device to produce a computer-implemented process, thereby providing the instructions executed on the computer or other programmable device for implementing the process. Figure 1 a process or multiple processes and / or boxes Figure 1 A step that specifies a function in one or more boxes.

[0091] Although the preferred embodiments of the present application have been described, those skilled in the art may make additional changes and modifications to these embodiments once they have learned the basic creative concept. Therefore, the appended claims are intended to be interpreted as including the preferred embodiments and all changes and modifications that fall within the scope of the present application.

[0092] Obviously, those skilled in the art may make various changes and modifications to this application without departing from the spirit and scope of this application. Thus, if these modifications and variations of this application fall within the scope of the claims of this application and their equivalents, this application is intended to include these modifications and variations.

Claims

1. A dynamic obstacle avoidance method for UAV based on safe flight corridor, characterized in that: include: S1: Construct a safe flight corridor model and a potential field constraint model, and plan the initial flight path for the UAV. The potential field constraint model is as follows: ； Where, represents the integrated potential field, represents the safe flight corridor boundary constraint potential field, represents the repulsive potential field of the dynamic obstacle, represents the attractive potential field at the target position, 、 、 They represent the weights of the safe flight corridor boundary constraint potential field, the dynamic obstacle repulsion potential field, and the target position attraction potential field respectively; S2: Control the drone to fly along the planned initial flight path and detect dynamic obstacles in real time. If the distance between the drone and the dynamic obstacle is less than the dynamic obstacle repulsion distance threshold, execute S3; S3: Calculate the expected acceleration of the UAV using the potential field constraint model and predict the positions of the UAV and the obstacle. Then, verify whether the predicted position of the UAV satisfies the safe flight corridor boundary constraint conditions and the safe distance constraint conditions from the obstacle. If not, increase the weight of the safe flight corridor boundary constraint potential field and / or the dynamic obstacle repulsion potential field accordingly until the predicted position of the UAV satisfies the constraints and the UAV is controlled to fly under the action of the comprehensive potential field. S4: Monitor whether the drone has completed obstacle avoidance. If so, control the drone to return to the planned initial flight path and continue flying.

2. The method for avoiding obstacles in a UAV in a safe flight corridor according to claim 1, wherein: In S1, ； Where, , indicating the current position of the drone To the center line of the corridor The shortest distance, is the boundary potential field coefficient, The radius of the corridor representing the safe flight corridor; ； Where, represents the obstacle repulsion coefficient, Indicates the distance threshold of the dynamic obstacle repulsion force, Indicates the drone and obstacles Dynamic distance; ； Where, is the attractive potential field coefficient, and Represent the positions of the UAV and the target respectively.

3. The method for avoiding obstacles in a UAV in a safe flight corridor according to claim 1, wherein: In S3, the expected acceleration of the drone The calculation formula is as follows: ； in, .

4. The method for dynamic obstacle avoidance of a UAV based on a safe flight corridor according to claim 3, characterized in that: In S3, the predicted position of the drone is calculated using the following formula: , where and Represent the predicted position and current position of the drone respectively, Indicates the speed of the drone, represents the desired acceleration of the drone; obstacle The predicted position is calculated using the following formula: , where and Represent obstacles The predicted and current locations of Indicates obstacles speed.

5. The method for dynamic obstacle avoidance of a UAV based on a safe flight corridor according to claim 3, characterized in that: In S3, if the predicted position of the UAV does not meet the safe flight corridor boundary constraint, the weight of the large safe flight corridor boundary constraint potential field is increased; if the predicted position of the UAV does not meet the safe distance constraint from the obstacle, the weight of the dynamic obstacle repulsion potential field is increased; if the predicted position of the UAV does not meet both the safe flight corridor boundary constraint and the safe distance constraint from the obstacle, the weight of the safe flight corridor boundary constraint potential field and the weight of the dynamic obstacle repulsion potential field are increased at the same time.

6. The method for dynamic obstacle avoidance of a UAV based on a safe flight corridor according to claim 5, characterized in that: If the predicted position of the UAV does not meet both the safe flight corridor boundary constraint and the safe distance constraint from the obstacle, the weight increase of the dynamic obstacle repulsion potential field is greater than the weight increase of the safe flight corridor boundary constraint potential field.

7. The method for dynamic obstacle avoidance of a UAV based on a safe flight corridor according to claim 1, characterized in that: In S4, the method for controlling the initial flight path of the UAV regression plan is as follows: Detect drones and any obstacles Are all satisfied ,in, is a safety margin. If it is satisfied, the potential field weight is readjusted and the UAV is made to fly to the initial flight path under the action of the comprehensive potential field after adjusting the potential field weight. In the comprehensive potential field, the weight of the target position attraction potential field is calculated by the following formula: ； The weight function of the potential field at the boundary of the safe flight corridor is calculated by the following formula: ； Where, = { , i=1,2,...,N}, represents the distance from the UAV to the nearest obstacle, , , , .

8. The method for dynamic obstacle avoidance of a UAV based on a safe flight corridor according to claim 7, characterized in that: In S4, the centerline of the safe flight corridor is set as the initial flight path, wherein the method for determining the return to the centerline of the safe flight corridor is as follows: Monitor in real time whether the distance between the current position of the drone and the center line of the safe flight corridor is close and whether the current speed direction of the drone is close to the expected speed direction. If so, determine that the drone has returned to the center line of the safe flight corridor.

Images