An effective obstacle avoidance method for UAVs tracking a preset flight path

Abstract

This invention proposes an effective obstacle avoidance method for unmanned aerial vehicles (UAVs) that tracks a preset flight path. This method addresses the technical problems of slow track recovery speed and large track deviation in existing technologies. The steps are as follows: 1) Calculate the coordinates of the UAV's track recovery point; 2) Define the equation of the internal common tangent between the track recovery turning circle and the threat circle; 3) Plan the UAV's obstacle avoidance segment track using an improved artificial potential field method; 4) Plan the UAV's track recovery segment track; 5) Obtain the UAV's obstacle avoidance result. This invention divides the entire obstacle avoidance process of the UAV into two segments: the obstacle avoidance segment track and the track recovery segment track, planning them separately. The UAV can fly along both segments sequentially and finally return to the preset flight path, improving the UAV's track recovery speed and reducing track deviation. This method can be applied to UAV collaborative search, communication relay, disaster relief, and other fields.

Description

Technical Field

[0001] This invention belongs to the field of UAV trajectory planning technology and relates to a UAV obstacle avoidance method, specifically a UAV obstacle avoidance method that effectively tracks a preset route, which can be used in UAV collaborative search, communication relay, disaster relief and other fields. Background Technology

[0002] Currently, drones are widely used in collaborative search, communication relay, and disaster relief due to their advantages of light weight, small size, low power consumption, high maneuverability, and low cost. During missions, drones are highly likely to collide with obstacles such as mountains and no-fly zones in the mission area, posing a threat to their safety. Therefore, when planning the drone's trajectory from the starting point to the target point, it is essential to consider whether obstacles in the environment will affect the drone's preset flight path, and thus implement obstacle avoidance trajectory planning. Drone obstacle avoidance refers to planning an ideal trajectory that avoids collisions with obstacles point by point, based on the drone's maneuverability constraints and the location and size of obstacles in the mission environment. In search scenarios, drones will cruise along a straight preset route covering the entire mission area. When encountering obstacles, they need to avoid them. Therefore, obstacle avoidance planning not only needs to consider obstacle avoidance but also the ability to quickly return to the preset route after obstacle avoidance, i.e., trajectory recovery. Improving the drone's trajectory recovery speed and reducing trajectory deviation are crucial for achieving effective tracking of the preset route.

[0003] Liao Jinyu et al. proposed a UAV obstacle avoidance method combining a preset flight path in their paper "Obstacle Avoidance Algorithm for Multi-rotor UAVs Combined with Preset Flight Path" (Sensors & Microsystems, 2017, 36(8): 152-154, 157). In this method, considering the task requirement of UAVs flying along a preset flight path, a linear potential field is introduced into the traditional artificial potential field method, combining the UAV's obstacle avoidance with flight along the preset flight path, so that the UAV can return to the flight path after avoiding obstacles and achieve tracking of the preset flight path. At the same time, the gravitational potential field function and the repulsive potential field function are improved, and finally a complete UAV obstacle avoidance and trajectory recovery path is obtained. However, its shortcomings are: since the UAV gradually returns to the preset flight path under the traction of the virtual force generated by the linear potential field after bypassing the obstacle, the trajectory recovery speed of the UAV is slow and the trajectory deviation is large. Summary of the Invention

[0004] The purpose of this invention is to address the shortcomings of the prior art by proposing an effective obstacle avoidance method for unmanned aerial vehicles (UAVs) that tracks a preset flight path. This method solves the technical problems in the prior art where the UAV's flight path recovery speed is slow and the flight path deviation is large because the virtual force generated by the linear potential field is used to guide the UAV around obstacles and gradually return to the preset flight path.

[0005] To achieve the above objectives, the technical solution adopted by the present invention includes the following steps:

[0006] (1) Calculate the coordinates of the UAV's track recovery point:

[0007] (1a) Initialize the minimum turning radius of the UAV in the Cartesian coordinate system xOy as r, the width of the UAV as 2w, and the UAV starts from the coordinate (x s ,y s The starting point S of ) to the coordinate (x g ,y g The straight line route formed by the target point G is SG, and the angle between SG and the x-axis in xOy is α; the center coordinates of obstacle P are (x p ,y p The radius of the threatening circle P' is R; the center coordinates of the threatening circle P' and the obstacle P are the same, and the radius is R+w; the track recovery turning circle with center J and radius r is located on the minor arc side where the threatening circle P' intersects the straight line SG, and the tangent points of this track recovery turning circle with SG and P' are H and Q, respectively; the intersection point of the line connecting the center of the threatening circle P' and the center of the track recovery turning circle with SG is N;

[0008] (1b) Calculate the coordinates (x, y) of the UAV track recovery point H using the lengths of line segments PN and JN, and the angle α between SG and the x-axis in xOy. H ,y H );

[0009] (2) Define the equation of the internal common tangent between the track recovery turning circle and the threat circle:

[0010] The equation of the internal common tangent l between the track recovery turning circle and the threat circle P', with the coordinates (x, y) of the track point to be substituted as unknowns, is defined as follows:

[0011] kx-y+(y Q -kx Q ) = 0

[0012]

[0013]

[0014]

[0015] Where k represents the slope of the internal common tangent l of the track recovery turning circle and the threat circle P', (x Q ,y Q (x) represents the coordinates of the point Q where the track recovery turning circle and the threat circle P' are tangent. J ,y J () indicates the coordinates of the center J of the turning circle for track recovery;

[0016] (3) Planning the obstacle avoidance trajectory of the UAV using the improved artificial potential field method:

[0017] The coordinates (x, y) of the drone's starting point S s ,y s Substituting into the equation of line l, calculate the positional relationship flag between the starting point S and the internal common tangent l, and use the improved artificial potential field method to plan an obstacle avoidance segment trajectory containing K waypoints based on flag, where:

[0018] flag = kx s -y s +(y Q -kx Q );

[0019] (4) Plan the flight path recovery segment of the UAV:

[0020] The track recovery segment formed by taking each track point in the obstacle avoidance segment as the starting point and the track recovery point H as the ending point is used as the planned Dubins path result. The bisection method is used to select the optimal Dubins path result among the K planned Dubins path results as the track recovery segment track.

[0021] (5) Obtain the obstacle avoidance results of the drone:

[0022] The drone follows a pre-set tracking path SG along a straight line, starting from coordinates (x... s ,y s Starting from point S, the flight path follows the obstacle avoidance section to the starting point of the track recovery section, then continues along the track recovery section to point H, and then flies along SG to the target point G, thus avoiding obstacle P.

[0023] Compared with the prior art, the present invention has the following advantages:

[0024] 1. This invention divides the entire obstacle avoidance process of the UAV into obstacle avoidance track and track recovery track, and plans them separately. The UAV flies along the two tracks in sequence and finally returns to the preset route. When planning the obstacle avoidance track, an improved artificial potential field method is used. The track is planned to stop when it comes into contact with the inner common tangent of the threat circle and the track recovery turning circle. Compared with the prior art, the obstacle avoidance track length is effectively shortened, thereby effectively improving the track recovery speed and reducing the track deviation.

[0025] 2. In planning the track recovery segment, this invention uses each track point in the obstacle avoidance segment as the starting point and the track recovery point as the ending point to form the track recovery segment track as the planned Dubins path result. It also uses a bisection method to select the optimal Dubins path result as the track recovery segment track. This overcomes the problem in the prior art that the UAV gradually returns to the preset route under the traction of the virtual force generated by the linear potential field after bypassing the obstacle. It further shortens the obstacle avoidance track length, thereby further improving the track recovery speed and reducing the track deviation. Attached Figure Description

[0026] Figure 1 This is a flowchart illustrating the implementation of the present invention;

[0027] Figure 2 This is a schematic diagram illustrating the calculation of the flight path recovery point coordinates of the UAV according to the present invention;

[0028] Figure 3 This is a schematic diagram showing the force relationship of the planned trajectory points in the potential field when the present invention plans the obstacle avoidance trajectory of a UAV using the improved artificial potential field method.

[0029] Figure 4 The optimal Dubins path result was selected as the schematic diagram of the track recovery segment for this invention;

[0030] Figure 5 A comparison diagram of obstacle avoidance trajectories planned by the present invention and existing technologies;

[0031] Figure 6 This is a comparison chart of the distance between the starting obstacle avoidance point and the obstacle, the distance between the track recovery point and the obstacle, and the track length during the obstacle avoidance phase in this invention and the prior art.

[0032] Figure 7 This is a comparison chart of the trajectory deviation between the obstacle avoidance trajectory of the present invention and the prior art. Detailed Implementation

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

[0034] Reference Figure 1 The present invention includes the following steps:

[0035] Step 1) Calculate the coordinates of the UAV's flight path recovery point, where the relationships between the preset straight flight path, obstacle and threat circles, and flight path recovery turning circles are as follows: Figure 2 As shown:

[0036] (1a) Since UAVs generally move on the same horizontal plane in space when performing missions, and their altitude changes are not frequent, a Cartesian coordinate system xOy is initialized. Considering the movement of the UAV in this two-dimensional coordinate system, the position of the UAV is represented by the two-dimensional coordinates projected from the air to the ground. In the coordinate system xOy, the minimum turning radius of the UAV is r, the width of the UAV is 2w, and the UAV starts from the coordinate (x s ,y s The starting point S of ) to the coordinate (x g ,y g The straight line route formed by the target point G is SG, and the angle between SG and the x-axis in xOy is α; the center coordinates of obstacle P are (x p ,y p The radius of the threat circle P' is R, and the center coordinates of the threat circle P' are the same as those of the obstacle P, with a radius of R+w. The drone and (x p ,y p The distance between the two points must not be less than R+w; otherwise, the drone will come into contact with the obstacle P, causing a collision and threatening the drone's safety. The threatening circle P' intersects with the straight path SG, creating a superior arc and a inferior arc. When the drone avoids obstacles, it will prioritize obstacle avoidance from the inferior arc side to shorten the track length. When it wants to restore the track, it will also return to the preset route from the inferior arc side. Therefore, a track restoration turning circle with center J and radius r is formed on the inferior arc side where the threatening circle P' intersects with the straight path SG. The tangent points of this track restoration turning circle with SG and P' are H and Q, respectively. The intersection point of the line connecting the center of the threatening circle P' and the center of the track restoration turning circle with SG is N.

[0037] (1b) Calculate the coordinates (x, y) of the UAV track recovery point H using the lengths of line segments PN and JN, and the angle α between SG and the x-axis in xOy. H ,y H ):

[0038]

[0039]

[0040]

[0041]

[0042]

[0043] The lengths of PN and JN are obtained by setting up equations based on the similarity relationship between triangles ΔPMN and ΔJHN.

[0044] Step 2) Define the equation of the internal common tangent between the track recovery turning circle and the threat circle:

[0045] The equation of the internal common tangent l between the track recovery turning circle and the threat circle P', with the coordinates (x, y) of the track point to be substituted as unknowns, is defined as follows:

[0046] kx-y+(y Q -kx Q ) = 0

[0047]

[0048]

[0049]

[0050] Where k represents the slope of the internal common tangent l of the track recovery turning circle and the threat circle P', (x Q ,y Q (x) represents the coordinates of the point Q where the track recovery turning circle and the threat circle P' are tangent. J ,y J () indicates the coordinates of the center J of the turning circle for track recovery;

[0051] Step 3) Plan the obstacle avoidance trajectory of the UAV using the improved artificial potential field method:

[0052] The coordinates (x, y) of the drone's starting point S s ,y s Substituting into the equation of line l, calculate the positional relationship between the starting point S and the internal common tangent line l (flag):

[0053] flag = kx s -y s +(y Q -kx Q )

[0054] Using the improved artificial potential field method, an obstacle avoidance segment trajectory containing K waypoints is planned based on the flag. The implementation steps are as follows:

[0055] (3a) Initialize the gravitational potential field coefficient as ξ, the repulsive potential field coefficient as η, the repulsive force action distance of the obstacle as d, the UAV speed as v, the total number of waypoints in the obstacle avoidance segment as K, and the coordinates of the k-th waypoint as (x k ,y k Let k = 1, x k =x s y k =y s ;

[0056] (3b) The force relationship of the k-th track point in the potential field is as follows: Figure 3 As shown, the distance from point S to point (x) k ,y kThe curve represents the planned obstacle avoidance section of the flight path consisting of the first k waypoints. The force exerted on the k-th waypoint in the potential field is calculated.

[0057]

[0058]

[0059]

[0060]

[0061]

[0062] in, and Let these represent the gravitational and repulsive forces experienced by the k-th track point in the potential field, respectively. Generated from target point G, and pointing from the k-th waypoint to target point G, the repulsive force... Generated by obstacle P, the path starts from the k-th waypoint and points in the opposite direction to obstacle P. Represents gravity Repulsive force The combined force and These represent the distances between the k-th waypoint and the target point G and the obstacle P, respectively.

[0063] Calculation here The gravity formula improves upon the traditional artificial potential field method's gravity formula by changing the gravitational force exerted by the target on the drone to a constant value, preventing the drone from colliding with obstacles due to excessive gravity when it encounters obstacles at a distance from the target point.

[0064] (3c) The force experienced by the vehicle in the potential field at the k-th track point Given the drone's speed v, calculate the drone's next waypoint q. next coordinates (x) next ,y next ):

[0065]

[0066] (3d) Move the drone to its next waypoint q next coordinates (x) next ,y next Substitute the equation of line l into the equation of line l, and calculate q. next Positional relationship with the internal common tangent l tmp k :

[0067] tmp k =kx next-y next +(y Q -kx Q )

[0068] (3e) Determine tmp k Is the sign different from flag? If yes, it indicates the waypoint q. next Point S is located on either side of the common tangent l of the track recovery turning circle and the threat circle P', respectively. At this point, the obstacle avoidance segment track planning is complete, resulting in an obstacle avoidance segment track containing K track points; otherwise, it indicates that track point q... next Let x be on the same side of the internal common tangent l as point S. k =x next y k =y next , k = k + 1, execute step (3b);

[0069] Step 4) Plan the drone's flight path recovery segment:

[0070] The track recovery segment formed by taking each track point in the obstacle avoidance segment as the starting point and the track recovery point H as the ending point is used as the planned Dubins path result. The bisection method is used to select the optimal Dubins path result among the K planned Dubins path results as the track recovery segment track.

[0071] Reference Figure 4 The curve from point S to point A represents the obstacle avoidance segment trajectory planned using the improved artificial potential field method in step 3). Points B and C are two track points in the obstacle avoidance segment trajectory. The Dubins paths planned from points B and C to point H are d1 and d2, respectively. Using the bisection method, the optimal Dubins path among the K planned Dubins paths is selected as the trajectory for the track recovery segment. The specific implementation is as follows:

[0072] Determine if the distances between all waypoints in the K / 2th Dubins path result of the K planned Dubins paths and the obstacle P are all greater than or equal to R+w: if so, ... Figure 4 As shown by curve d1, continue binary search within the first K / 2 Dubins path results to find the optimal Dubins path result; otherwise, as... Figure 4 As shown by curve d2, we continue to perform binary search on the last K / 2 Dubins path results to find the optimal Dubins path result.

[0073] Step 5) Obtain the obstacle avoidance results of the drone:

[0074] The drone follows a pre-set tracking path SG along a straight line, starting from coordinates (x... s ,y sStarting from point S, the flight path follows the obstacle avoidance section to the starting point of the track recovery section, then continues along the track recovery section to point H, and then flies along SG to the target point G, thus avoiding obstacle P.

[0075] This invention divides the entire obstacle avoidance process of a UAV into an obstacle avoidance track and a track recovery track. An improved artificial potential field method is used to plan the obstacle avoidance track. A bisection method is employed to select the optimal result from the Dubins paths formed by each track point in the obstacle avoidance track as the starting point and the track recovery point H as the ending point, which is then used as the track recovery track. The UAV flies along both tracks sequentially and finally returns to the preset route. In contrast, existing technologies only perform obstacle avoidance track planning once. After avoiding obstacles, the UAV gradually returns to the preset route under the traction of the virtual force generated by the linear potential field. Compared with existing technologies, this invention has a faster track recovery speed and a smaller track deviation.

[0076] The effects of this invention will be further illustrated below with simulation experiments:

[0077] 1. Simulation conditions and content:

[0078] Hardware environment: CPU is Intel(R) Core(TM) i5-10400 with a clock speed of 2.90GHz, memory is 24.0GB, and operating system is 64-bit.

[0079] Software environment: Microsoft Windows 10, Python 3.7.8 programming environment, PyCharm Community Edition 2021.1.1 simulation software, Matlab R2022a simulation software.

[0080] Simulation parameter settings: The simulation parameter settings for the UAV flight environment, its own maneuverability, and obstacle avoidance methods are shown in Table 1.

[0081] Table 1 Simulation parameter settings for UAV flight environment, maneuverability, and obstacle avoidance methods

[0082] parameter Value <![CDATA[Center coordinates of obstacle P1]]> (900，1000)m <![CDATA[Radius r1 of obstacle P1]]> 300m <![CDATA[Center coordinates of the obstacle P2]]> (2400，2150)m <![CDATA[Radius r2 of obstacle P2]]> 300m UAV starting point S coordinate (200，100)m UAV target point G coordinate (3200，3100)m Drone speed v 20m / s Minimum turning radius r of drone 100m drone width 2w 40m The gravitational potential coefficient ξ of the method of this invention <![CDATA[10 -5 ]]> The repulsive potential coefficient η of the method of the present invention 800 The repulsive force application distance d of the method of the present invention 500m Existing technology gravitational potential field coefficient <![CDATA[5×10 -5 ]]> Existing technology repulsive potential field coefficient <![CDATA[5×10 5 ]]> Existing technology line gravitational potential field coefficient <![CDATA[2×10 -6 <!-- 6 -->]]> Existing technology repulsive force range 500m

[0083] Simulation 1: Under the above environment and parameters, obstacle avoidance planning was performed on the actual obstacle avoidance trajectory of the UAV using both the present invention and existing technologies. The simulation results are compared as follows: Figure 5 As shown.

[0084] Simulation 2 compares the present invention and the prior art in three aspects: the distance from the starting obstacle avoidance point to the obstacle, the distance from the track recovery point to the obstacle, and the total length of the track during the obstacle avoidance phase. The simulation results are as follows: Figure 6 As shown.

[0085] Simulation 3 compares the trajectory deviation of the present invention and the prior art. The simulation results are as follows: Figure 7 As shown in the figure. Here, track deviation represents the average distance from all track points on the actual planned track from the starting point S to the target point G to the preset route.

[0086] 2. Simulation Result Analysis:

[0087] refer to Figure 5 Two obstacles, P1 and P2, are set between the starting point S and the target point G, with their centers located to the upper left and lower right of the preset flight path SG, respectively. Both obstacles intersect SG at points M1 and N1, and M2 and N2, respectively. The solid lines from point A1 to point B1 and from point A2 to point B2 represent the UAV obstacle avoidance paths planned by the method of this invention for obstacles P1 and P2, respectively. The dashed lines from point C1 to point D1 and from point C2 to point D2 represent the UAV obstacle avoidance paths planned by the prior art for obstacles P1 and P2, respectively. Figure 5 It is evident that the track recovery points B1 and B2 obtained by the method of the present invention are closer to the obstacle, indicating that the track recovery speed of the method of the present invention is faster.

[0088] refer to Figure 6 For two obstacles P1 and P2, the distances from the initial obstacle avoidance point to the obstacles are denoted as A1M1 and C1M1, A2M2 and C2M2, respectively; the distances from the track recovery point to the obstacles are denoted as B1N1 and D1N1, B2N2 and D2N2, respectively; and the total track length during the obstacle avoidance phase is denoted as A1B1 and C1D1, A2B2 and C2D2, respectively. For example... Figure 6 As shown, for obstacle P1, compared with the prior art, the method of this invention shortens the total length of the obstacle avoidance phase by approximately 164m; the distance from the starting obstacle avoidance point to the obstacle by 18m; and the distance from the track recovery point to the obstacle by approximately 244m. For obstacle P2, the three distances are shortened by approximately 132m, 20m, and 229m respectively. The data comparison shows that the starting obstacle avoidance point and the track recovery point of the method of this invention are closer to the obstacle, and the overall obstacle avoidance track length is shorter, effectively improving the UAV's track recovery speed and enabling the UAV to return to the preset route more quickly.

[0089] refer to Figure 7The trajectory deviations of the method of this invention and the prior art are 44.23m and 60.84m, respectively. The trajectory deviation of the planned trajectory of the method of this invention is reduced by approximately 27.3% compared with the prior art. Trajectory deviation is an important indicator characterizing the effectiveness of UAV in tracking a preset route. Therefore, simulation results show that, compared with the prior art, the method of this invention effectively reduces trajectory deviation by planning the obstacle avoidance segment and the trajectory recovery segment separately, resulting in better UAV tracking of the preset route.

Claims

1. A method for obstacle avoidance of a UAV effectively tracking a preset flight path, characterized in that, The method comprises the following steps: (1) calculating the coordinates of the track recovery point of the unmanned aerial vehicle: (1a) initialize the minimum turning radius of the UAV in the plane rectangular coordinate system xOy as r, the machine width as 2w, and the UAV flies from the starting point S with coordinates (x s ,y s ) to the target point G with coordinates (x g ,y g ), and the straight-line course formed is SG, the angle between SG and the x-axis in xOy is α; the center coordinates of the obstacle P are (x p ,y p ), and the radius is R; the threat circle P' has the same center coordinates as the obstacle P, and the radius is R+w; the flight path recovery turning circle with the center J and the radius r is located on the inferior arc side of the intersection of the threat circle P' and the straight-line course SG, and the tangent points of the flight path recovery turning circle and SG and P' are H and Q respectively; the intersection of the line connecting the center of the threat circle P' and the center of the flight path recovery turning circle and SG is N; (1b) Calculate the coordinates (x H ,y H ) of the UAV track recovery point H by the lengths of the line segments PN and JN, and the included angle a of SG with the x-axis in xOy; (2) defining the internal tangent line equation of the track recovery turning circle and the threat circle: The equation of the internal tangent line l of the track recovery turning circle and the threat circle P' with the track point coordinates (x, y) to be substituted as unknowns is defined as: kx-y + (y Q -kx Q ) = 0 where k represents the slope of the inner tangent line 1 of the track recovery turn circle and the threat circle P', (x Q ,y Q ) represents the coordinates of the tangent point Q of the track recovery turn circle and the threat circle P', and (x J ,y J ) represents the coordinates of the center J of the track recovery turn circle. (3) planning the track of the obstacle avoidance segment of the unmanned aerial vehicle by using the improved artificial potential field method: The coordinates (x s ,y s ) of the starting point S of the unmanned aerial vehicle are substituted into the straight line equation of l, the positional relationship flag of the starting point S and the inner tangent line l is calculated, and an improved artificial potential field method is used to plan an obstacle avoidance segment flight path containing K flight path points according to the flag, wherein: flag = kx s - y s + (y Q - kx Q ); (4) planning the track of the track recovery segment of the unmanned aerial vehicle: The track recovery segment track formed by taking each track point in the obstacle avoidance segment track as a starting point and taking the track recovery point H as an ending point is taken as the planned Dubins path result, and the dichotomy is used to select the optimal Dubins path result in the K planned Dubins path results as the track recovery segment track; (5) obtaining the obstacle avoidance result of the unmanned aerial vehicle: The UAV flies along the straight line SG as a tracking preset route, starts from the starting point S with coordinates (x s ,y s ), flies along the obstacle avoidance segment flight path to the starting point of the flight path of the flight path recovery segment, continues to fly along the flight path of the flight path recovery segment to the H point, and then flies along SG to the target point G, thereby achieving obstacle avoidance for the obstacle P.

2. The method of claim 1, wherein: The coordinates (x H ,y H ) of the UAV path recovery point H in step (1b) are calculated according to the following formula:

3. The method of claim 1, wherein: In step (3), the improved artificial potential field method is used to plan the obstacle avoidance segment track containing K track points according to the flag, and the implementation step is as follows: (3a) initializing the coefficient of the attractive potential field as ξ, the coefficient of the repulsive potential field as η, the repulsive action distance of the obstacle as d, the speed of the unmanned aerial vehicle as v, the total number of the track points of the track of the obstacle avoidance segment as K, the coordinates of the kth track point as (x k ,y k ), and setting k = 1, x k =x s , y k =y s ; (3b) calculating the force experienced by the kth waypoint in the potential field wherein, and Gkand Rkrepresent the attractive and repulsive forces respectively experienced by the kth waypoint in the potential field, and Gkand Rkrepresent the distances from the kth waypoint to the goal point G and the obstacle P respectively. (3c) Force experienced by the kth waypoint in the potential field With the drone velocity v, the coordinates (x next ,y next ) of the next waypoint q of the drone are calculated: next ​ (3d) Substituting the coordinates (x next ,y next ) of the next waypoint q next of the UAV into the straight line equation of l, the positional relationship tmp next of q k with the internal tangent l is calculated. tmp k = kx next - y next + (y Q - kx Q ) (3e) judge tmp k and whether the flags are of different signs: if yes, the obstacle-avoiding segment track containing K track points is obtained; otherwise, let x k = x next , y k = y next , k = k + 1, and execute step (3b).

4. The method of claim 1, wherein: In step (4), the dichotomy is used to select the optimal Dubins path result in the K planned Dubins path results as the track recovery segment track, and the implementation step is as follows: It is judged whether the distances between all the track points in the K / 2th Dubins path result in the K planned Dubins path results and the obstacle P are greater than or equal to R+w: if yes, the dichotomy is continued to be used to find the optimal Dubins path result in the first K / 2 Dubins path results; otherwise, the dichotomy is continued to be used to find the optimal Dubins path result in the last K / 2 Dubins path results.

Images