A method for drone obstacle avoidance to quickly recover its flight path

Abstract

This invention discloses a method for rapid track recovery for unmanned aerial vehicles (UAVs) to avoid obstacles, primarily addressing the problems of slow track recovery speed and poor recovery effect in existing UAV obstacle avoidance methods. The solution involves: 1) transforming the known starting point, target point, and obstacles from the global coordinate system to a local coordinate system; 2) determining the UAV's turning direction in the local coordinate system; 3) enlarging the obstacle radius based on the UAV's minimum turning radius and the obstacle's radius; 4) calculating the coordinates of key track points for obstacle avoidance in the local coordinate system based on the geometric relationship between the UAV and the obstacle; 5) transforming the coordinates of the key track points in the local coordinate system to the global coordinate system, allowing the UAV to fly around the obstacle sequentially past all key track points, completing obstacle avoidance and rapid track recovery. This invention significantly shortens the track length during the obstacle avoidance phase and improves the track recovery speed of the UAV after avoiding obstacles, making it suitable for UAV collaborative reconnaissance, pinpoint strikes, and communication relay.

Description

Technical Field

[0001] This invention belongs to the field of UAV trajectory planning, and in particular relates to a UAV obstacle avoidance method, which can be used for UAV collaborative reconnaissance, pinpoint strike, and communication relay. Background Technology

[0002] Currently, drones are widely used in collaborative reconnaissance, precision strikes, and communication relay scenarios due to their advantages of low power consumption, light weight, small size, high maneuverability, and low cost. In many mission scenarios, because drones share airspace with obstacles such as no-fly zones and mountains, collisions with these obstacles are highly likely during mission execution, threatening drone safety. Therefore, when planning a drone's flight path from its starting point to its target point, it is essential to consider whether environmental obstacles will affect the drone's flight path, and thus implement obstacle avoidance flight path planning. Drone obstacle avoidance refers to planning an ideal flight path that avoids collisions by changing the drone's flight state. In reconnaissance scenarios, drones patrol the mission area along a predetermined offline flight path, covering the entire area. When encountering obstacles, they need to avoid them. Therefore, obstacle avoidance planning not only needs to consider obstacle avoidance but also requires the drone to quickly return to its original offline flight path after obstacle avoidance to continue patrolling. Improving the drone's flight path recovery speed and shortening the overall flight path length during the obstacle avoidance phase are crucial for improving the drone's coverage of the mission area.

[0003] Representative obstacle avoidance methods for unmanned aerial vehicles (UAVs) mainly include: obstacle avoidance methods based on artificial potential fields, obstacle avoidance methods based on the Restricted Tracking Method (RRT) algorithm, and obstacle avoidance and trajectory recovery methods based on heading control laws. Among these, the artificial potential field-based method establishes a virtual potential field and guides the UAV to avoid obstacles and reach the target point based on the direction of the resultant force. The RRT algorithm-based method introduces randomness in path search, randomly generating waypoints that satisfy the condition of obstacle avoidance, thus achieving obstacle avoidance. The heading control law-based obstacle avoidance and trajectory recovery method considers the trajectory recovery problem, calculating key waypoints in the UAV's obstacle avoidance process based on the heading control law, enabling the UAV to avoid obstacles and achieve trajectory recovery.

[0004] In her master's thesis, "Research and Implementation of Real-Time Route Planning Algorithm for Multiple UAVs" (Xi'an: Xi'an University of Electronic Science and Technology, 2018), Guo Jiaorong proposed a UAV obstacle avoidance and trajectory recovery method based on heading control law. The method models obstacles as two-dimensional circular planes, describing them using the center and radius. First, it uses the vector cross product method to determine the UAV's obstacle avoidance turning direction. Then, it calculates the turning angle using the point-to-point turning method. Next, it calculates the key track points on the obstacle avoidance path and obtains the coordinates of each key track point in the trajectory recovery stage based on symmetry, ultimately obtaining the complete UAV obstacle avoidance and trajectory recovery trajectory. The drawback of this method is that the UAV's starting obstacle avoidance track point is too far from the obstacle, and the trajectory recovery speed is slow, resulting in an excessively long trajectory for the entire obstacle avoidance segment. Summary of the Invention

[0005] The purpose of this invention is to address the shortcomings of the prior art by proposing a method for drone obstacle avoidance that allows for rapid recovery of the drone's trajectory, thereby accelerating the recovery speed of the drone's trajectory after bypassing obstacles and shortening the drone's trajectory during the entire obstacle avoidance phase.

[0006] The technical solution of this invention is: fully considering the environmental constraints and the drone's own maneuverability constraints during obstacle avoidance, namely the coordinates of the starting point S, the target point G, the center of the obstacle P, and the obstacle radius r, and the drone's minimum turning radius R. min Flight speed v, minimum track length l min The process involves transforming the coordinates of the starting point, target point, and obstacles from the global coordinate system to a local coordinate system. This allows for the determination of the turning direction during obstacle avoidance by the UAV. Furthermore, it calculates key waypoints during obstacle avoidance based on the geometric relationship between the UAV and obstacles. Finally, all the coordinates of these waypoints are transformed back to the global coordinate system, resulting in a smooth, flyable obstacle avoidance path with rapid recovery. The implementation steps include the following:

[0007] (1) Given the known starting point S(x) S ,y S ), target point G(x) G ,y G ), obstacle P(x) P ,y P Transform from the global coordinate system to the local coordinate system:

[0008] (1a) Set the starting point S(x) S ,y S Using the origin of the local coordinate system as the reference point, the global coordinate system is translated so that the origin of the global coordinate system coincides with the starting point S.

[0009] (1b) From the starting point S(x) S ,y S ) and target point (x)G ,y G Determine the vector pointing from the starting point S to the target point G.

[0010] (1c) will vector Let θ be the angle between the x-axis and the positive x-axis of the global coordinate system. Rotate the global coordinate system counterclockwise by θ so that the positive x-axis is perpendicular to the vector... By coinciding, a local coordinate system is obtained;

[0011] (1d) Calculate the coordinates S'(x') of the starting point S, the target point G, and the obstacle P in the local coordinate system. S ,y' S ), G'(x' G ,y' G ), P'(x' P ,y' P );

[0012] (2) Determine the turning direction of the UAV in the local coordinate system:

[0013] If the ordinate value of the obstacle's center is greater than 0, it is determined that the center is located on the left side of the straight flight path, and the drone will bypass the obstacle from the right side.

[0014] If the ordinate value of the obstacle's center is less than or equal to 0, it is determined that the center is located on the right side of the straight flight path or on the straight flight path, and the UAV will bypass the obstacle from the left side to avoid it.

[0015] (3) Adjust the radius of the obstacle:

[0016] Let the minimum turning radius of the drone be R. min And compare it with the sum of the obstacle radius r and half the drone width w:

[0017] If R min If ≤r+w, which satisfies the minimum turning radius constraint of the UAV, then the obstacle avoidance trajectory of the UAV is planned according to the radius r+w;

[0018] If R min If the radius of the obstacle is greater than r+w, it does not satisfy the minimum turning radius constraint of the UAV, so the obstacle radius r is increased to R. min –w, according to radius R min Plan the obstacle avoidance flight path for drones;

[0019] (4) Calculate the coordinates of the key waypoints for UAV obstacle avoidance in the local coordinate system:

[0020] (4a) The entire process of the UAV from the start of obstacle avoidance to the return to the original track is divided into three stages: obstacle avoidance stage, detour stage and track recovery stage;

[0021] (4b) Calculate the waypoints for obstacle avoidance by the UAV at each stage:

[0022] During the obstacle-circling phase, the path point C'(x') when the UAV begins its obstacle-circling flight after the straight flight ends is obtained using the bisection method. C ,y' C ), and calculate the track point D' when the circumnavigation ends and the straight flight begins:

[0023] (x' D ,y' D )=(2x' P -x' C ,y' C ),

[0024] Where (x' D ,y' D () represents the coordinates of track point D';

[0025] During the obstacle avoidance phase, the track point A' when the drone begins to avoid obstacles and makes a turn, and the track point B' when the drone finishes the turn and begins to fly straight again are calculated:

[0026]

[0027] (x' B ,y' B )=(x' C -l min ·cosα,y' C +l min ·sinα)

[0028] Among them, (x' A ,y' A (x') represents the coordinates of track point A', (x') B ,y' B ) represents the coordinates of track point B', l min This represents the minimum path segment length constraint for the UAV, (x' H ,y' H ) represents the coordinates of the intersection point H' of the line perpendicular to P'C' passing through point C' and intersecting the x-axis, and α represents the angle between H'C' and the positive x-axis;

[0029] During the track recovery phase, the track point E' when the UAV finishes its straight flight and begins to turn to return to its original track, and the track point F' when it finishes its turn and returns to its original track are calculated:

[0030] (x' E ,y' E )=(2x' P -x' B ,y' B )

[0031] (x' F ,y' F )=(2x' P -x' A ,y' A )

[0032] Among them, (x' E ,y' E (x') represents the coordinates of track point E', (x') F ,y' F ) represents the coordinates of track point F';

[0033] (5) Transform the coordinates of the key track points A', B', C', D', E', and F' calculated in the local coordinate system in (4b) to the global coordinate system to obtain the coordinates of track points A, B, C, D, E, and F.

[0034] (6) The UAV flies over the waypoints A, B, C, D, E and F in sequence to complete obstacle avoidance and rapid waypoint recovery.

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

[0036] 1. When determining the turning direction of a UAV, this invention can determine the turning direction of the UAV simply by comparing whether the ordinate of the center of the obstacle in the local coordinate system is greater than 0. Compared with the complex method of using vector cross product for UAV obstacle avoidance and track recovery based on the heading control law, this invention not only eliminates the need for calculation but is also simple and intuitive.

[0037] 2. This invention comprehensively considers the constraints of obstacles in the environment and the maneuverability of the UAV itself. It uses the geometric relationship between the UAV and the obstacle to calculate the key track points in the obstacle avoidance process. This makes the starting obstacle avoidance point and the track recovery point closer to the obstacle, which greatly shortens the track length in the obstacle avoidance phase and improves the track recovery speed of the UAV after bypassing the obstacle. Attached Figure Description

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

[0039] Figure 2 This is a schematic diagram of the rotating coordinate system in this invention;

[0040] Figure 3 This is a schematic diagram illustrating the determination of the turning direction of the drone in this invention;

[0041] Figure 4 This is a schematic diagram of obstacle avoidance and flight path recovery for the UAV in this invention;

[0042] Figure 5 This is a schematic diagram illustrating the use of the bisection method to solve for the coordinates of key waypoints in this invention.

[0043] Figure 6 The images show obstacle avoidance paths planned using the present invention and existing obstacle avoidance methods based on heading control laws, respectively.

[0044] Figure 7 for Figure 6 A comparison chart of the distances between the starting obstacle avoidance point and the obstacle, the distances between the track recovery point and the obstacle, and the track lengths during the obstacle avoidance phase in two different obstacle avoidance paths. Detailed Implementation

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

[0046] Reference Figure 1 The implementation steps for the instance are as follows:

[0047] Step 1, coordinate transformation.

[0048] 1.1) Obtain the starting point S(x) of the UAV. S ,y S ), target point G(x) G ,y G ), obstacle P(x) P ,y P These are the known coordinates;

[0049] 1.2) Set the starting point S(x) S ,y S Using the origin of the local coordinate system as the reference point, the global coordinate system is translated so that the origin of the global coordinate system coincides with the starting point S.

[0050] 1.3) Starting from the initial point S(x) S ,y S ) and target point G(x) G ,y G Determine the vector pointing from the starting point S to the target point G.

[0051] 1.4) Vector Let θ be the angle between the x-axis and the positive x-axis of the global coordinate system. Rotate the global coordinate system counterclockwise by θ so that the positive x-axis is perpendicular to the vector... By coinciding, a local coordinate system is obtained;

[0052] 1.5) Calculate the coordinates S'(x') of the starting point S, target point G, and obstacle P in the local coordinate system. S ,y' S ), G'(x' G ,y' G ), P'(x' P ,y' P ):

[0053] Reference Figure 2 In this step, the point S'(x') is calculated in the local coordinate system. S ,y' S ), G'(x' G ,y' G ), P'(x' P ,y' P The coordinates of ) are as follows:

[0054]

[0055]

[0056]

[0057] Where d1 represents the distance from point S to vector... The distance from point T on the vector, d2 represents the distance of the vector. Let d1 represent the distance from point T to point U, d2 represent the distance from point P to point V, and d3 represent the distance from point U to point V.

[0058] Step 2: Determine the turning direction of the drone in the local coordinate system.

[0059] This step is based on the ordinate value of the center of the obstruction:

[0060] If the ordinate value of the obstacle's center is greater than 0, it is determined that the center is located on the left side of the straight flight path, and the drone will bypass the obstacle from the right side.

[0061] If the ordinate value of the obstacle's center is less than or equal to 0, it is determined that the center is located on the right side of the straight flight path or on the straight flight path, and the UAV will bypass the obstacle from the left side to avoid it.

[0062] like Figure 3 As shown, in the local coordinate system, if the ordinate value of the obstacle center P1 is greater than 0, it is determined that the center is located on the left side of the straight track, and the UAV avoids the obstacle from the right side along l1; if the ordinate value of the obstacle center P2 is less than 0, it is determined that the center P2 is located on the right side of the straight track, and the UAV avoids the obstacle from the left side along l2.

[0063] Step 3: Process the radius of the obstacle.

[0064] Let the minimum turning radius of the drone be R. min And compare it with the sum of the obstacle radius r and half the drone width w:

[0065] If R min If ≤r+w, which satisfies the minimum turning radius constraint of the UAV, then the obstacle avoidance trajectory of the UAV is planned according to the radius r+w;

[0066] If Rmin If the radius of the obstacle is greater than r+w, it does not satisfy the minimum turning radius constraint of the UAV, so the obstacle radius r is increased to R. min –w, according to radius R min Plan the obstacle avoidance flight path for drones.

[0067] Step 4: Calculate the coordinates of the key waypoints for UAV obstacle avoidance in the local coordinate system.

[0068] 4.1) The entire process of the UAV from the start of obstacle avoidance to returning to its original flight path is divided into three stages: obstacle avoidance stage, detour stage, and flight path recovery stage;

[0069] like Figure 4 As shown, points A' and F' are the obstacle avoidance start point and the track recovery point, respectively. The process from point A' through point B' to point C' is called the obstacle avoidance phase, the process from point C' to point D' is called the UAV detour phase, and the process from point D' through point E' to point F' is called the track recovery phase.

[0070] Among them, the arc and arc For R min The trajectory is a circular arc segment with radius r, and B'C' and D'E' are straight line segments that are tangent to the circle with center P' and radius r+w. The trajectory is a circular arc segment with the center point P' of the obstacle as the center and r+w as the radius.

[0071] 4.2) Calculate the waypoints for obstacle avoidance by the UAV at each stage;

[0072] 4.2.1) During the obstacle-circling phase, the path point C'(x') when the UAV begins its obstacle-circling flight after the straight flight ends is obtained using the bisection method. C ,y' C ):

[0073] Reference Figure 5 The specific implementation of this step is as follows:

[0074] First, the drone starts from point A' and moves along the arc. With point J' as the center, R min Let the radius be the radius for the turn, and point Q' lie on line segment C'H', with J'Q' perpendicular to C'H'; let point M'(x') be the radius for the turn. M ,y' M Let N'(x') be the point closer to the origin among the two intersection points of a circle with center P' and radius r+w with the x-axis. N ,y' N () is the midpoint of the minor arc formed by the intersection of a circle with center P' and radius r+w and the x-axis. The coordinates of these two points are calculated as follows:

[0075]

[0076] (x' N ,y' N )=(x' P ,y' P -rw);

[0077] Next, the bisection method is used on the arc. Continuously select points as points C'(x') C ,y' C Draw a perpendicular line from point C' to P'C', intersecting the x-axis at point H'. Let the size of ∠M'H'C' be α. Along the vector... Take a point B' on line segment C'H' such that B'C' = l min Given H'B' = H'C' - B'C', calculate the coordinates of points H' and B':

[0078]

[0079] (x' B ,y' B )=(x' C -l min ·cosα,y' C +l min ·sinα)

[0080] Among them, l min This represents the minimum flight segment length constraint for the UAV;

[0081] Next, H'B' / R min With H'Q' / R min Comparison:

[0082] like Figure 5 As shown in (a), point B' lies on line segment Q'C', and Q'C' > l. min The optimal solution at point C' should lie on the arc. Up, then continue on the arc. The optimal point C' is found using the bisection method.

[0083] like Figure 5 As shown in (b), point B' lies on line segment H'Q', and Q'C' < l. min The optimal solution at point C' should lie on the arc. Up, then continue on the arc. The optimal point C' is found using the bisection method.

[0084] like If point B' coincides with point Q', then the bisection stops, and the point C'(x') at this point... C ,y' C This is the optimal solution;

[0085] Then, after obtaining the coordinates of point C', calculate the track point D' when the round trip ends and the straight flight begins:

[0086] (x' D ,y' D )=(2x' P -x' C ,y' C )

[0087] Among them, (x' D ,y' D () represents the coordinates of track point D';

[0088] 4.2.2) During the obstacle avoidance phase, calculate the coordinates of the UAV's track point A' when it begins obstacle avoidance and turns, and the track point B' when it ends the turn and begins straight flight:

[0089]

[0090] (x' B ,y' B )=(x' C -l min ·cosα,y' C +l min ·sinα)

[0091] Among them, (x' A ,y' A (x') represents the coordinates of track point A', (x') B ,y' B (x') represents the coordinates of track point B', (x') H ,y' H ) represents the coordinates of the intersection point H' of the line perpendicular to P'C' passing through point C' and intersecting the x-axis, and α represents the angle between H'C' and the positive x-axis;

[0092] 4.2.3) During the track recovery phase, calculate the track point E' when the UAV finishes its straight flight and begins to turn to return to its original track, and the track point F' when it finishes its turn and returns to its original track:

[0093] (x' E ,y' E )=(2x' P -x' B ,y' B )

[0094] (x' F ,y'F )=(2x' P -x' A ,y' A )

[0095] Among them, (x' E ,y' E (x') represents the coordinates of track point E', (x') F ,y' F ) represents the coordinates of track point F'.

[0096] Step 5: Transform the coordinates of the key track points A', B', C', D', E', and F' calculated in the local coordinate system in Step 4 to the global coordinate system, thus obtaining the corresponding coordinates (x, y) of track points A, B, C, D, E, and F. A ,y A ), (x B ,y B ), (x C ,y C ), (x D ,y D ), (x E ,y E ), (x F ,y F ):

[0097] (x A ,y A )=(x' A cosθ-y' A sinθ+x S ,y' A cosθ+x' A sinθ+y S ),

[0098] (x B ,y B )=(x' B cosθ-y' B sinθ+x S ,y' B cosθ+x' B sinθ+y S ),

[0099] (x C ,y C )=(x' C cosθ-y' C sinθ+x S ,y' C cosθ+x' C sinθ+y S ),

[0100] (x D ,y D )=(x' D cosθ-y' D sinθ+x S ,y' D cosθ+x' D sinθ+y S ),

[0101] (x E ,y E )=(x' E cosθ-y' E sinθ+x S ,y' E cosθ+x' E sinθ+y S ),

[0102] (x F ,y F )=(x' F cosθ-y' F sinθ+x S ,y' F cosθ+x' F sinθ+y S ).

[0103] Step 6: The drone flies over waypoints A, B, C, D, E, and F in sequence to complete obstacle avoidance and rapid track recovery.

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

[0105] 1. Simulation parameter settings:

[0106] The simulation parameters for the UAV's flight environment and its own maneuverability are shown in Table 1:

[0107] Table 1 Simulation parameters for UAV flight environment and its own maneuverability

[0108] parameter Value Task scene size 3300m×3300m <![CDATA[Center coordinates of obstacle P1]]> (900，1000)m <![CDATA[Radius r1 of obstacle P1]]> 300m <![CDATA[Center coordinates of obstacle P2]]> (2500，2400)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 <![CDATA[Minimum turning radius R of the drone min > 100m <![CDATA[Minimum track segment length l of the drone min > 200m drone width 2w 40m

[0109] 2. Simulation content and result analysis:

[0110] Simulation 1: Under the aforementioned environment and parameters, obstacle avoidance planning for the actual obstacle avoidance trajectory of the UAV was performed using both the present invention and existing UAV obstacle avoidance and trajectory recovery methods based on heading control laws. The results are as follows: Figure 6 The solid lines represent the UAV obstacle avoidance trajectory planned by the method of this invention, and the dotted lines represent the UAV obstacle avoidance trajectory planned by the method based on the heading control law.

[0111] like Figure 6 As can be seen, since obstacle P1 is located above the straight track, both methods choose to bypass the obstacle by going around its minor arc from below. However, since obstacle P2 is located exactly on the straight track, both methods choose to go around it from above.

[0112] Simulation 2 compares and evaluates the present invention and existing UAV obstacle avoidance and trajectory recovery methods based on heading control laws from three aspects: the distance from the initial obstacle avoidance point to the obstacle, the distance from the trajectory recovery point to the obstacle, and the total trajectory length during the obstacle avoidance phase. The results are as follows: Figure 7 In this context, for the two obstacles P1 and P2, the distances from the starting obstacle avoidance point to the obstacles are represented by A1K1 and A1'K1, A2K2 and A2'K2, respectively; the distances from the track recovery point to the obstacles are represented by F1T1 and F1'T1, F2T2 and F2'T2, respectively; and the total track length during the obstacle avoidance phase is represented by A1F1 and A1'F1', A2F2 and A2'F2', respectively.

[0113] from Figure 7 As can be seen, for obstacle P1, the total length of the trajectory during the obstacle avoidance phase is shortened by nearly 1000m compared to the comparative method; the distance from the starting obstacle avoidance point to the obstacle and the distance from the trajectory recovery point to the obstacle are both shortened by nearly 500m. For obstacle P2, the three distances are shortened by approximately 716m, 363m, and 363m respectively.

[0114] The simulation results above show that the UAV obstacle avoidance method for rapid track recovery proposed in this invention can make both the starting obstacle avoidance point and the track recovery point closer to the obstacle, greatly shortening the track length during the obstacle avoidance phase and improving the track recovery speed of the UAV after bypassing the obstacle.

Claims

1. A method for rapid track recovery obstacle avoidance for unmanned aerial vehicles (UAVs), characterized in that, Includes the following steps: (1) Given the known starting point S(x) S ,y S ), target point G(x) G ,y G ), obstacle P(x) P ,y P Transform from the global coordinate system to the local coordinate system: (1a) Set the starting point S(x) S ,y S Using the origin of the local coordinate system as the reference point, the global coordinate system is translated so that the origin of the global coordinate system coincides with the starting point S. (1b) From the starting point S(x) S ,y S ) and target point (x) G ,y G Determine the vector pointing from the starting point S to the target point G. (1c) will vector Let θ be the angle between the x-axis and the positive x-axis of the global coordinate system. Rotate the global coordinate system counterclockwise by θ so that the positive x-axis is perpendicular to the vector... By coinciding, a local coordinate system is obtained; (1d) Calculate the coordinates S'(x') of the starting point S, the target point G, and the obstacle P in the local coordinate system. S ,y' S ), G'(x' G ,y' G ), P'(x' P ,y' P ); (2) Determine the turning direction of the UAV in the local coordinate system: If the ordinate value of the obstacle's center is greater than 0, it is determined that the center is located on the left side of the straight flight path, and the drone will bypass the obstacle from the right side. If the ordinate value of the obstacle's center is less than or equal to 0, it is determined that the center is located on the right side of the straight flight path or on the straight flight path, and the UAV will bypass the obstacle from the left side to avoid it. (3) Adjust the radius of the obstacle: Let the minimum turning radius of the drone be R. min And compare it with the sum of the obstacle radius r and half the drone width w: If R min If ≤r+w, which satisfies the minimum turning radius constraint of the UAV, then the obstacle avoidance trajectory of the UAV is planned according to the radius r+w; If R min If the radius of the obstacle is greater than r+w, it does not satisfy the minimum turning radius constraint of the UAV, so the obstacle radius r is increased to R. min –w, according to radius R min Plan the obstacle avoidance flight path for drones; (4) Calculate the coordinates of the key waypoints for UAV obstacle avoidance in the local coordinate system: (4a) The entire process of the UAV from the start of obstacle avoidance to the return to the original track is divided into three stages: obstacle avoidance stage, detour stage and track recovery stage; (4b) Calculate the waypoints for obstacle avoidance by the UAV at each stage: During the obstacle-circling phase, the path point C'(x') when the UAV begins its obstacle-circling flight after the straight flight ends is obtained using the bisection method. C ,y' C ), and calculate the track point D' when the circumnavigation ends and the straight flight begins: (x' D ,y' D )=(2x' P -x' C ,y' C ), Where (x' D ,y' D () represents the coordinates of track point D'; During the obstacle avoidance phase, the track point A' when the drone begins to avoid obstacles and makes a turn, and the track point B' when the drone finishes the turn and begins to fly straight again are calculated: (x' B ,y' B )=(x' C -l min ·cosα,y' C +l min ·sinα) Among them, (x' A ,y' A (x') represents the coordinates of track point A', (x') B ,y' B ) represents the coordinates of track point B', l min This represents the minimum path segment length constraint for the UAV, (x' H ,y' H ) represents the coordinates of the intersection point H' of the line perpendicular to P'C' passing through point C' and intersecting the x-axis, and α represents the angle between H'C' and the positive x-axis; During the track recovery phase, the track point E' when the UAV finishes its straight flight and begins to turn to return to its original track, and the track point F' when it finishes its turn and returns to its original track are calculated: (x' E ,y' E )=(2x' P -x' B ,y' B ) (x' F ,y' F )=(2x' P -x' A ,y' A ) Among them, (x' E ,y' E (x') represents the coordinates of track point E', (x') F ,y' F ) represents the coordinates of track point F'; (5) Transform the coordinates of the key track points A', B', C', D', E', and F' calculated in the local coordinate system in (4b) to the global coordinate system to obtain the coordinates of track points A, B, C, D, E, and F. (6) The UAV flies over the waypoints A, B, C, D, E and F in sequence to complete obstacle avoidance and rapid waypoint recovery.

2. The method according to claim 1, characterized in that: In step (1d), the coordinates of points S, G, and P in the global coordinate system are calculated, and their coordinates S'(x') in the local coordinate system are calculated. S ,y' S ), G'(x' G ,y' G ), P'(x' P ,y' P The formula is as follows:

3. The method according to claim 1, characterized in that: In step (4b), during the obstacle-circling phase, the bisection method is used to obtain the track point C'(x') when the UAV finishes its straight flight and begins to circle the obstacle. C ,y' C The implementation is as follows: (4b1) Let the coordinates of the obstacle P in the global coordinate system be P'(x') after transformation to the local coordinate system. P ,y' P Let M'(x') be the point closer to the origin among the two intersection points of the circle with center P' and radius r+w and the straight track. M ,y' M Let the midpoint of the minor arc resulting from the intersection be denoted as point N'(x'). N ,y' N The coordinates of these two points are described as follows: The coordinates of point M' are: The coordinates of point N' are based on y' P Whether it is greater than 0 is determined in the following two cases: If y' P If the x-axis is greater than 0, then the coordinates of point N' are: (x') N ,y' N )=(x' P ,y' P -rw) If y' P If ≤0, then the coordinates of point N' are: (x' N ,y' N )=(x' P ,y' P +r+w) (4b2) Solving for the path point C'(x') using the bisection method C ,y' C ): Using the bisection method on the arc Continuously select points as C'(x') C ,y' C Draw a perpendicular line from point C' to P'C', intersecting the x-axis at point H'. Let the size of ∠M'H'C' be α. Along the vector... Take a point B' on line segment C'H' such that B'C' = l min Given H'B' = C'H' - B'C', calculate the coordinates (x', y') of point H'. H ,y' H The coordinates (x') of point B' and point B' B ,y' B ): (x' B ,y' B )=(x' C -l min ·cosα,y' C +l min ·sinα); Suppose the drone starts from point A' and travels along the arc... With point J' as the center, R min Make a turn with radius, and point Q' lies on line segment C'H', J'Q' is perpendicular to C'H', and H'B' / R min With H'Q' / R min Comparison: like Continue on the arc The optimal point C' is found using the bisection method. like Continue on the arc The optimal point C' is found using the bisection method. like Stop the binary search; at this point, point C'(x') C ,y' C That is the optimal solution.

4. The method according to claim 1, characterized in that: In step (5), the coordinates of the key track points A', B', C', D', E', and F' in the local coordinate system are transformed to the global coordinate system, thus obtaining the coordinates (x, y) of track points A, B, C, D, E, and F. A ,y A ), (x B ,y B ), (x C ,y C ), (x D ,y D ), (x E ,y E ), (x F ,y F The formula is as follows: (x A ,y A )=(x' A cosθ-y' A sinθ+x S ,y' A cosθ+x' A sinθ+y S ) (x B ,y B )=(x' B cosθ-y' B sinθ+x S ,y' B cosθ+x' B sinθ+y S ) (x C ,y C )=(x' C cosθ-y' C sinθ+x S ,y' C cosθ+x' C sinθ+y S ) (x D ,y D )=(x' D cosθ-y' D sinθ+x S ,y' D cosθ+x' D sinθ+y S ) (x E ,y E )=(x' E cosθ-y' E sinθ+x S ,y' E cosθ+x' E sinθ+y S ) (x F ,y F )=(x' F cosθ-y' F sinθ+x S ,y' F cosθ+x' F sinθ+y S )。

Images