Unmanned aerial vehicle trajectory planning method

Abstract

The invention relates to the technical field of unmanned aerial vehicles, in particular to an unmanned aerial vehicle trajectory planning method, which is characterized by comprising the following steps of: 1, acquiring a starting point of current planning, judging whether a trajectory still exists behind a current waypoint, if so, executing the next step, and if not, ending route planning; step 2, acquiring a planning track of this time, judging the type of a current waypoint, and if the current waypoint is a starting point and the starting point is a circling point, executing step 3; and step 3, if the current waypoint is an end point, executing a circling starting point mode 1, if the current waypoint is not the end point and the next waypoint is a circling point, executing step 3-1, and if the current waypoint is not the end point and the next waypoint is not the circling point, executing step 3-2. The method is at least beneficial to improving the consistency of the actual flight path of the unmanned aerial vehicle and the ground station planning path.

Description

Technical Field

[0001] This invention belongs to the field of unmanned aerial vehicle (UAV) technology, and in particular relates to a UAV trajectory planning method. Background Technology

[0002] Currently, existing UAV ground stations can only display the trajectory of two adjacent waypoints in a straight line when performing path planning. However, fixed-wing UAVs cannot arbitrarily change their flight direction during flight and need to detour before returning to the original trajectory. As a result, the actual flight trajectory of the UAV does not match the trajectory planned by the ground station. Summary of the Invention

[0003] In view of this, the present invention aims to provide a method for planning the trajectory of unmanned aerial vehicles (UAVs), which at least helps to improve the consistency between the actual flight trajectory of the UAV and the planned trajectory of the ground station.

[0004] To achieve the above objectives, the technical solution created by this invention is implemented as follows: This invention provides a method for UAV trajectory planning, comprising: Step 1, obtaining the starting point of the current plan, determining whether a trajectory still exists after the current waypoint; if yes, proceeding to the next step; otherwise, ending the route planning; Step 2, obtaining the current planned trajectory, determining the type of the current waypoint; if the current waypoint is the starting point and the starting point is a hovering point, proceeding to Step 3; Step 3, if the current waypoint is the ending point, executing hovering starting point mode 1; if the current waypoint is not the ending point and the next waypoint is a hovering point, executing Step 3-1; if the current waypoint is not the ending point and the next waypoint is not a hovering point, executing Step 3-2; Step 3-1, determining whether the distance between the current waypoint and the next waypoint meets a first preset condition; if it does, executing hovering starting point mode 2; if it does not, continuing to determine whether the directions of the preceding and following hovering points are the same; if they are the same... If the two conditions are not met, then the circling starting point mode 4 is executed. If they are not the same, then the distance between the current waypoint and the next waypoint is checked to see if it meets the second preset condition. If it does, then the circling starting point mode 4 is executed; otherwise, the circling starting point mode 6 is executed. In step 3-2, the distance between the current waypoint and the next waypoint is checked to see if it meets the third preset condition. If it does, then the circling starting point mode 2 is executed; otherwise, the circling starting point mode 5 is executed. After executing circling starting point mode 1, circling starting point mode 2, circling starting point mode 3, circling starting point mode 4, circling starting point mode 5, or circling starting point mode 6, the current planned endpoint is obtained. The current planned endpoint is used as the starting point for the next planning and the process returns to step 1.

[0005] Compared with the prior art, the present invention can achieve the following beneficial effects: The UAV trajectory planning method provided by the present invention can rationally plan the flight trajectory according to the type of waypoint, the positional relationship between waypoints and the UAV's turning radius, so that the UAV can fly along the planned trajectory and avoid the phenomenon that the actual flight trajectory of the UAV does not match the planned trajectory of the ground station. Attached Figure Description

[0006] The accompanying drawings, which form part of this invention, are used to provide a further understanding of the invention. The illustrative embodiments and descriptions of the invention are used to explain the invention and do not constitute an undue limitation of the invention. In the drawings: Figure 1 A partial flowchart of the UAV trajectory planning method described in the embodiments of the present invention; Figure 2 A flowchart illustrating the UAV trajectory planning method described in this embodiment of the invention, where the current waypoint is the starting point; Figure 3 The flowchart of the UAV trajectory planning method described in the embodiments of the present invention, with the current waypoint as the destination; Figure 4 A flowchart illustrating the UAV trajectory planning method described in this embodiment of the invention, where the current waypoint is the intermediate point; Figure 5 A flowchart of the common midpoint step in the UAV trajectory planning method described in the embodiments of the present invention; Figure 6 A flowchart of the hovering midpoint step in the UAV trajectory planning method described in the embodiments of the present invention; Figure 7 A flowchart of the hovering endpoint step in the UAV trajectory planning method described in the embodiments of the present invention; Figure 8 A schematic diagram of hovering starting point mode 1 in the UAV trajectory planning method described in the embodiment of the present invention; Figure 9 A schematic diagram of hovering start point mode 2 when the subsequent waypoint is a hovering point in the UAV trajectory planning method described in the embodiment of the present invention; Figure 10 A schematic diagram of hovering starting point mode 2 when the subsequent waypoints are ordinary waypoints in the UAV trajectory planning method described in the embodiment of the present invention; Figure 11 A schematic diagram of hovering starting point mode 3 when the subsequent waypoints are ordinary waypoints in the UAV trajectory planning method described in the embodiment of the present invention; Figure 12 A schematic diagram of the hovering starting point mode 4 when the hovering directions are the same in the UAV trajectory planning method described in the embodiment of the present invention; Figure 13 A schematic diagram of hovering starting point mode 4 when the hovering directions are different in the UAV trajectory planning method described in the embodiment of the present invention; Figure 14 A schematic diagram of hovering starting point mode 5 in the UAV trajectory planning method described in the embodiment of the present invention; Figure 15 A schematic diagram of hovering starting point mode 6 in the UAV trajectory planning method described in the embodiment of the present invention; Figure 16 A schematic diagram of the ordinary waypoint endpoint mode in the UAV trajectory planning method described in the embodiments of the present invention; Figure 17 A schematic diagram of hovering endpoint mode 1 in the UAV trajectory planning method described in the embodiment of the present invention; Figure 18 A schematic diagram of hovering endpoint mode 2 in the UAV trajectory planning method described in the embodiment of the present invention; Figure 19 A schematic diagram of hovering endpoint mode 3 in the UAV trajectory planning method described in the embodiment of the present invention; Figure 20 A schematic diagram of the ordinary midpoint mode 1 in the UAV trajectory planning method described in the embodiment of the present invention; Figure 21 A schematic diagram of the ordinary midpoint mode 2 in the UAV trajectory planning method described in the embodiment of the present invention; Figure 22 A schematic diagram of the ordinary midpoint mode 3 in the UAV trajectory planning method described in the embodiment of the present invention; Figure 23 A schematic diagram of the ordinary midpoint pattern 4 in the UAV trajectory planning method described in the embodiment of the present invention; Figure 24 A schematic diagram of the ordinary midpoint pattern 5 in the UAV trajectory planning method described in the embodiment of the present invention; Figure 25 A schematic diagram of the ordinary midpoint pattern 6 in the UAV trajectory planning method described in the embodiment of the present invention; Figure 26 A schematic diagram of the hovering midpoint transition mode 1 in the UAV trajectory planning method described in the embodiment of the present invention when the subsequent waypoint is a hovering point; Figure 27 A schematic diagram of the hovering midpoint transition mode 1 when the subsequent waypoint is a normal waypoint in the UAV trajectory planning method described in the embodiment of the present invention; Figure 28A schematic diagram of the hovering midpoint transition mode 2 in the UAV trajectory planning method described in the embodiment of the present invention; Figure 29 A schematic diagram of the midpoint transition mode 3 when the hovering directions are the same in the UAV trajectory planning method described in the embodiment of the present invention; Figure 30 A schematic diagram of the hovering midpoint transition mode 3 when the hovering direction is different in the UAV trajectory planning method described in the embodiment of the present invention; Figure 31 A schematic diagram of the hovering midpoint transition mode 4 in the UAV trajectory planning method described in the embodiment of the present invention; Figure 32 A schematic diagram of the hovering midpoint transition mode 5 in the UAV trajectory planning method described in the embodiment of the present invention; Figure 33 A schematic diagram of the jump-into-mode 1 in the UAV trajectory planning method described in the embodiment of the present invention; Figure 34 This is a schematic diagram illustrating the transition from a normal point to mode 2 in the UAV trajectory planning method described in an embodiment of the present invention. Figures 8 to 34 In the diagram, the red line segment represents the currently planned path, and the blue line segment represents the subsequent marked path. Figure 35 A trajectory planning diagram obtained by the UAV trajectory planning method described in the present invention embodiment; Figure 36 Another trajectory planning diagram obtained by the UAV trajectory planning method described in the present invention embodiment; Figure 37 According to Figure 35 The actual flight trajectory is obtained by performing actual flight on the trajectory planning map; Figure 38 According to Figure 36 The actual flight trajectory is obtained by performing actual flight on the trajectory planning map. Detailed Implementation

[0007] To make the objectives, technical solutions, and advantages of this invention clearer, the invention will be further described in detail below with reference to the accompanying drawings and specific embodiments. It should be understood that the specific embodiments described herein are merely illustrative of the invention and do not constitute a limitation thereof.

[0008] It should be noted that, unless otherwise specified, the embodiments and features described in the present invention can be combined with each other.

[0009] In the description of this invention, it should be understood that the terms "center," "longitudinal," "lateral," "length," "width," "thickness," "upper," "lower," "front," "rear," "left," "right," "vertical," "horizontal," "top," "bottom," "inner," "outer," "clockwise," and "counterclockwise," etc., indicating orientations or positional relationships based on the orientations or positional relationships shown in the accompanying drawings, are only for the convenience of describing this invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation, and therefore should not be construed as a limitation on this invention. Furthermore, the terms "first," "second," etc., are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of indicated technical features. Thus, features defined with "first," "second," etc., may explicitly or implicitly include one or more of that feature. In the description of this invention, unless otherwise stated, "a plurality of" means two or more.

[0010] In the description of this invention, it should be noted that, unless otherwise explicitly specified and limited, the terms "installation," "connection," and "linking" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral connection; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; and they can refer to the internal connection of two components. Those skilled in the art will understand the specific meaning of the above terms in this invention based on the specific circumstances.

[0011] The invention will now be described in detail with reference to the accompanying drawings and embodiments.

[0012] refer to Figures 1 to 38 This invention provides a method for planning the trajectory of an unmanned aerial vehicle (UAV), comprising: refer to Figure 1 and Figure 2 Step 1: Obtain the starting point of the current plan, and determine whether there is still a trajectory after the current waypoint. If yes, proceed to the next step; otherwise, end the route planning. Step 2: Obtain the planned trajectory and determine the type of the current waypoint. It should be noted that the current waypoint can be a starting point, an ending point, or an intermediate point. The starting point can be a hovering point or a regular waypoint. The ending point can be a jump point, a hovering point, or a regular waypoint. The intermediate point can be a jump point, a hovering point, or a regular waypoint. A hovering point is a point where you hover with a fixed radius. A jump point is a point where you jump to a previous waypoint. A regular waypoint is a waypoint that is neither a hovering point nor a jump point.

[0013] If the current waypoint is the starting point and the starting point is a hovering point, then proceed to step 3; Step 3: If the current waypoint is the ending point, then proceed to hovering starting point mode 1; if the current waypoint is not the ending point and the next waypoint is a hovering point, then proceed to step 3-1; if the current waypoint is not the ending point and the next waypoint is not a hovering point, then proceed to step 3-2. Step 3-1: Determine whether the distance between the current waypoint and the next waypoint meets the first preset condition. If it does, execute the hovering start mode 2. If it does not, continue to determine whether the directions of the previous and next hovering points are the same. If they are the same, execute the hovering start mode 4. If they are not the same, determine whether the distance between the current waypoint and the next waypoint meets the second preset condition. If it does, execute the hovering start mode 4. If it does not, execute the hovering start mode 6. Step 3-2: Determine whether the distance between the current waypoint and the next waypoint meets the third preset condition. If it does, execute the hovering start mode 2. If it does not, continue to determine whether the distance between the current waypoint and the next waypoint meets the fourth preset condition. If it does, execute the hovering start mode 3. If it does not, execute the hovering start mode 5. After executing hovering start mode 1, hovering start mode 2, hovering start mode 3, hovering start mode 4, hovering start mode 5, or hovering start mode 6, the current planning endpoint is obtained. The current planning endpoint is used as the starting point for the next planning and the process returns to step 1.

[0014] Further reference Figure 1 and Figure 2 After determining the type of the current waypoint in step 2, the following steps are also taken: if the current waypoint is the starting point and the starting point is a normal waypoint, then continue to determine the type of the next waypoint. If the next waypoint is a hovering point, then execute step 4. If the next waypoint is a normal waypoint, then add a normal waypoint mark to both the current waypoint and the next waypoint. The planned trajectory of the two normal waypoints is to go directly from the current waypoint to the next waypoint.

[0015] Step 4 includes: determining whether the distance between the current waypoint and the next waypoint meets the fifth preset condition. If it does, add ordinary waypoint markers to both the current waypoint and the next waypoint. If it does not meet the condition, mark the waypoint to enter the next waypoint from the current waypoint in a tangential manner.

[0016] The first presupposition condition is: AB > 2 × R A ×cos(asin(0.5))+2×R B ×cos(asin(0.5)); The second preset condition is: AB>R A +R B The third presupposition condition is: AB > 2 × R A ×cos(asin(0.5)); The fourth preset condition is: AB>R AThe fifth presupposition is: AB > 2 × R B ×cos(asin(0.5)) or AB <R B AB represents the length of the line connecting the current waypoint and the next waypoint, R A R represents the turning radius of the current waypoint. B The turning radius represents the next waypoint.

[0017] It should be noted that after adding ordinary waypoint markers to both the current waypoint and the next waypoint, or after entering the next waypoint by tangent, the next waypoint is used as the endpoint of the current plan, and the endpoint of the current plan is used as the starting point of the next plan, and the process returns to step 1.

[0018] Further reference Figure 1 and Figure 3 After determining the type of the current waypoint in step 2, the following steps are also included: if the current waypoint is the destination and the destination is a hovering point, then step 5 is executed; if the current waypoint is the destination and the destination is a normal point, then the normal waypoint destination mode is executed. Step 5 includes: determining whether the hovering point is entered tangentially. If not, it continues to determine whether the distance between the current waypoint and the next waypoint meets the sixth preset condition. If it does, hovering endpoint mode 1 is executed; otherwise, hovering endpoint mode 2 is executed. If the point is entered tangentially, hovering endpoint mode 3 is executed. The sixth preset condition is: AB > 2 × R. B ×cos(asin(0.5)), where AB represents the length of the line connecting the current waypoint and the next waypoint, and R B The turning radius represents the next waypoint.

[0019] It should be noted that after executing the normal waypoint destination mode, the hovering destination mode 1, the hovering destination mode 2, or the hovering destination mode 3, the current planned destination is obtained. The current planned destination is used as the starting point for the next planning and the process returns to step 1.

[0020] Further reference Figure 1 and Figure 4Step 2, after determining the type of the current waypoint, also includes: If the current waypoint is an intermediate point, and the intermediate point is a jump point, then determine whether the current waypoint is a hovering point; if so, execute the hovering endpoint step, and after the hovering endpoint step is completed, execute the hovering midpoint step, and after the hovering midpoint step is completed, continue to determine whether the jump target point is a hovering point; if the target point is a hovering point, then execute the hovering endpoint step again, and after the hovering endpoint step is completed, execute the hovering midpoint step, and after the hovering midpoint step is completed, end the current planning, obtain the endpoint of the current planning, and return to step 1; if the target point is not a hovering point, then determine whether the first characteristic is met. If a certain condition is met, jump to mode 1; otherwise, jump to mode 2. If the current waypoint is not a hovering point, execute the normal midpoint step. After the normal midpoint step is completed, check if the target point is a hovering point. If the target point is a hovering point, execute the hovering endpoint step. After the hovering endpoint step is completed, execute the hovering midpoint step. After the hovering midpoint step is completed, end the current planning, obtain the endpoint of the current planning, and return to step 1. If the target point is not a hovering point, check if the first specific condition is met. If it is met, jump to mode 1; otherwise, jump to mode 2. If the current waypoint is an intermediate point, and the intermediate point is a hovering point instead of a jump point, then execute the hovering end point step. After the hovering end point step is completed, execute the hovering midpoint step. After the hovering midpoint step is completed, end the current planning, obtain the end point of the current planning, and return to step 1. If the current waypoint is an intermediate point, and the intermediate point is not a jump point but a regular point, then execute the regular midpoint step. After the regular midpoint step is completed, end the current planning, obtain the destination of the current planning, and return to step 1.

[0021] Further reference Figure 7 The hovering endpoint steps include: determining whether the hovering point is entered tangentially; if not, determining whether the distance between the current waypoint and the next waypoint meets the sixth preset condition; if it does, executing hovering endpoint mode 1; if not, executing hovering endpoint mode 2; and if the hovering point is entered tangentially, executing hovering endpoint mode 3.

[0022] refer to Figure 6The hovering midpoint step includes: determining whether the next waypoint is a hovering point; if so, proceed to step 5; otherwise, proceed to step 6. Step 6 includes: determining whether the distance between the current waypoint and the next waypoint meets the seventh preset condition; if so, proceed to hovering midpoint transition mode 1; if not, proceed to determine whether the distance between the current waypoint and the next waypoint meets the eighth preset condition; if so, proceed to hovering midpoint transition mode 2; if not, proceed to hovering midpoint transition mode 4. Step 5 includes: determining whether the distance between the current waypoint and the next waypoint meets the ninth preset condition; if so, proceed to hovering midpoint transition mode 1; if not, proceed to determine whether the directions of the preceding and following hovering points are the same; if so, proceed to hovering midpoint transition mode 3; if not, proceed to determine whether the distance between the current waypoint and the next waypoint meets the tenth preset condition; if so, proceed to hovering midpoint transition mode 3; if not, proceed to hovering midpoint transition mode 5.

[0023] The sixth presupposition condition is: AB > 2 × R B ×cos(asin(0.5)); The seventh preset condition is: AB>2×R B ×cos(asin(0.5)); The eighth preset condition is: AB>R B The ninth presupposition is: AB > 2 × 2 × R B ×cos(Asin(0.5)); The tenth preset condition is: AB>2×R B AB represents the length of the line connecting the current waypoint and the next waypoint, R B The turning radius represents the next waypoint.

[0024] Further reference Figure 5 The normal midpoint procedure includes: determining whether the distance between the current waypoint and the next waypoint meets the eleventh preset condition; if so, executing normal midpoint mode 1; otherwise, continuing to determine whether the distance between the current waypoint and the next waypoint, and the distance between the next waypoint and the waypoint after that, meets the twelfth preset condition; if so, executing normal midpoint mode 2; if not, continuing to determine whether the second specific condition is met; if so, executing normal midpoint mode 3; if not, continuing to determine whether the third specific condition is met; if so, executing normal midpoint mode 4; if not, continuing to determine whether the fourth specific condition is met; if so, executing normal midpoint mode 5; if not, executing normal midpoint mode 6. In some embodiments, the eleventh preset condition is: AB and BC simultaneously satisfy greater than R / (tan(∠ABC / 2)), and ∠ABC is greater than 60°; the twelfth preset condition is: AB and BC simultaneously satisfy greater than 2×R×sin(acos(R×sin(∠ABC / 2))+R) / (2×R)))+R×cos(∠ABC / 2)), where AB represents the length of the line connecting the current waypoint and the next waypoint, and BC represents the length of the line connecting the next waypoint and the waypoint after that; after executing Normal Midpoint Mode 1, Normal Midpoint Mode 2, Normal Midpoint Mode 3, Normal Midpoint Mode 4, Normal Midpoint Mode 5, or Normal Midpoint Mode 6, the endpoint of the current plan is obtained, and the endpoint of the current plan is used as the starting point of the next plan and the process returns to step 1.

[0025] Furthermore, given a turning radius of R, we define A as the current waypoint and B as the next waypoint.

[0026] refer to Figure 8 The starting point mode 1 of the hovering is: the trajectory is to hover with A as the center and R as the hovering radius.

[0027] refer to Figure 9 and Figure 10 The second mode for the starting point of the hovering is: when B is the hovering point, refer to... Figure 9 Given the line AB connecting A and B, determine points Q1 and Q21 on AB, satisfying AQ1 = 2 × R × cos(asin(0.5)) and BQ21 = 2 × R × cos(asin(0.5)). Determine the perpendicular line O1Q1 to AB, where O1Q1 = R. The line connecting O1 and A intersects a circle with center A and radius R at point Q2. Determine the perpendicular line O21Q21 to AB, where O21Q21 = R. The line connecting O21 and B intersects a circle with center B and radius R at point Q3. The trajectory is a spiraling circle with center A and radius R. Figure 9 The trajectory is clockwise. After circling to point Q2, it circles around O1 with a radius of R back to Q1. Q1 is the currently planned endpoint. The subsequent trajectory is from Q1 along AB to Q21, then circles around O21 with a radius of R from Q21 to Q3. From Q3, it circles around B with a radius of R. When B is a normal waypoint, refer to... Figure 10 Given the line AB connecting A and B, determine a point Q1 on AB such that AQ1 = 2 × R × cos(asin(0.5)). Determine the perpendicular line O1Q1 from AB, where O1Q1 = R. The line connecting O1 and A intersects a circle with center A and radius R at point Q3. The trajectory is a circle that spirals around center A with radius R. Figure 10 The trajectory is clockwise. After reaching point Q3, it will circle around point O1 with radius R to reach point Q1. Q1 is the current planned endpoint. The subsequent trajectory is from Q1 along AB to B.

[0028] refer to Figure 11 The third spiraling starting point pattern is as follows: A point Q1 is determined on a circle with center A and radius R. Q1 satisfies the condition that Q1B is perpendicular to Q1A. The trajectory is a spiraling path with center A and radius R leading to point Q1. Figure 11 The path rotates clockwise, with Q1 being the current planned endpoint. The subsequent trajectory starts from Q1 and proceeds along Q1B to B.

[0029] refer to Figure 12 and Figure 13 The fourth mode of the hovering starting point is: when B and A hover in the same direction, refer to... Figure 12 Determine Q1 on a circle centered at A and with radius R, and determine Q2 on a circle centered at B and with radius R, where Q1Q2 = AB and is parallel to AB. The trajectory is a circle centered at A and with radius R, spiraling towards point Q1, with Q1 as the current planned endpoint. The subsequent trajectory starts from Q1, follows Q1Q2 to Q2, and then spirals from Q2 again, centered at B and with radius R. Figure 12 Both A and B rotate clockwise; when B and A rotate in different directions, refer to... Figure 13 Determine the midpoint Q3 of AB. On a circle with center A and radius R, determine Q1. On a circle with center B and radius R, determine Q2. Q1Q3 is perpendicular to Q1A, and Q2Q3 is perpendicular to Q2B. The trajectory is a clockwise spiral around center A and radius R to point Q1. Q1 is the current planned endpoint. The subsequent trajectory is to start from Q1, follow Q1Q2 to Q2, and then start from Q2, spiraling counterclockwise around center B and radius R.

[0030] refer to Figure 14 The starting point pattern 5 is as follows: Determine Q1 and O1 on a circle with center A and radius R. Q1 is located on the extension of AB, AO1=R, and AO1 is perpendicular to AQ1. Determine Q2 on a circle with center O1 and radius R. Q1Q2 is perpendicular to AQ1, and Q1Q2=R. The trajectory is to rotate clockwise with center A and radius R. After rotating to Q1, cut out and reach Q2 along Q1Q2. Then, starting from Q2, rotate clockwise with center O1 and radius R. After rotating to A, take the position of A as the current planned endpoint. The subsequent trajectory is to start from A and reach B along AB.

[0031] refer to Figure 15The starting point pattern 6 is as follows: The line AB intersects with a circle centered at A and with radius R at Q1. O1 is determined on the extension of AB, and O1Q1=R. Q2 is determined on the circle centered at B and with radius R, and Q2B is perpendicular to AB. Q3 is determined on the circle centered at O1 and with radius R, and Q3Q2 is perpendicular to Q2B. The trajectory is to spiral clockwise with A as the center and R as the radius, spiral to Q1, and then spiral counterclockwise with O1 as the center and R as the radius to Q3. Q3 is the currently planned endpoint. The subsequent trajectory is to start from Q3, follow Q3Q2 to Q2, and then spiral counterclockwise with B as the center and R as the radius starting from Q2.

[0032] Furthermore, given a turning radius of R, we define A as the current waypoint and B as the next waypoint.

[0033] refer to Figure 16 The normal waypoint destination mode is as follows: the trajectory starts from A and proceeds along AB to B, where B is the currently planned destination.

[0034] refer to Figure 17 The first spiraling endpoint pattern is as follows: Determine point Q1 on AB, satisfying BQ1=2×R×cos(asin(0.5)), determine the perpendicular line O1Q1 of AB, O1Q1=R, the line connecting O1 and B intersects the circle with B as the center and R as the radius at point Q3, the trajectory is to start from A and go along AQ1 to Q1, then spiral from Q1 to Q3 with O1 as the center and R as the radius, and then spiral clockwise from Q3 with B as the center and R as the spiral radius.

[0035] refer to Figure 18 The second spiraling endpoint pattern is as follows: Determine Q1 and O1 on a circle with B as the center and R as the radius. Q1 is located on the extension of BA, BO1=R, and BO1 is perpendicular to BQ1. Determine Q2 on a circle with O1 as the center and R as the radius. Q1Q2=R, and Q1Q2 is perpendicular to Q1B. The trajectory is to start from A and go along AB to B. From B, spiral clockwise with O1 as the center and R as the radius to Q2. Cut out from Q2 and go along Q2Q1 to Q1. Then, start from Q1 and spiral clockwise with B as the center and R as the radius.

[0036] refer to Figure 19 The third spiraling endpoint pattern is as follows: Q1 is determined on a circle with B as the center and R as the radius. Q1 satisfies that Q1B is perpendicular to Q1A. The trajectory is to start from A and go along AQ1 to Q1. Then, starting from Q1, the spiraling pattern is clockwise with B as the center and R as the radius.

[0037] Furthermore, given a turning radius of R, define A as the current waypoint, B as the next waypoint, and C as the next waypoint after B; (Refer to...) Figure 33The jump to mode 1 is as follows: On the side of BC facing A, determine O, satisfying that OB is perpendicular to BC and OB=R; on the side of AB away from C, determine O1; and on AB, determine Q1, satisfying that Q1O1 is perpendicular to AB and Q1O1=R, OO1=2×R. Obtain the midpoint Q3 of Q1O. The trajectory is to start from A and go along AB to Q1. Starting from Q1, rotate counterclockwise with O1 as the center and R as the radius to Q3. Then, starting from Q3, rotate clockwise with O as the center and R as the radius to B. B is the current planned endpoint. The subsequent trajectory is to start from B and go along BC to C. The first specific condition is: Q1 is between AB.

[0038] refer to Figure 34 The transition to mode 2 is as follows: On the side of AB away from C, determine O1, satisfying that O1B is perpendicular to AB and O1B=R. On the side of BC away from A, determine O, satisfying that OB is perpendicular to BC and OB=R. Determine Q1 on a circle with O1 as the center and R as the radius, and determine Q2 on a circle with O as the center and R as the radius, satisfying that Q1Q2 is parallel to OO1. The trajectory is from A along AB to B, from B, rotating counterclockwise with O1 as the center and R as the radius to Q1, from Q1 along Q1Q2 to Q2, from Q2, rotating counterclockwise with O as the center and R as the radius to B. At this point, B is the current planned endpoint, and the subsequent trajectory is from B along BC to C.

[0039] refer to Figure 26 and Figure 27 The transition mode 1 at the midpoint of the hovering point is: when C is the hovering point, refer to... Figure 26 Determine point B and the endpoint A' of the hovering endpoint mode. Given the line BC connecting B and C, determine points Q1 and Q21 on BC, satisfying BQ1 = 2 × R × cos(asin(0.5)) and CQ21 = 2 × R × cos(asin(0.5)). Determine the perpendicular line O1Q1 to BC, O1Q1 = R. The line connecting O1 and B intersects the circle with B as center and R as radius at point Q2. Determine the perpendicular line O21Q21 to BC. 21=R, the line connecting O21 and C intersects the circle with C as the center and R as the radius at Q3. The trajectory starts from A', with B as the center and R as the radius, and rotates clockwise to point Q2. Then, starting from point Q2, it rotates with O1 as the center and R as the radius to Q1. Q1 is the current planned endpoint. The subsequent trajectory starts from Q1 and moves along BC to Q21. Then, with O21 as the center and R as the radius, it rotates from Q21 to Q3. Starting from Q3, it rotates clockwise with C as the center and R as the rotation radius. When C is a normal waypoint, refer to Figure 27Determine point B and the endpoint A' of the spiraling endpoint mode. Given the line BC connecting B and C, determine point Q1 on BC, satisfying BQ1=2×R×cos(asin(0.5)). Determine the perpendicular line O1Q1 of BC, O1Q1=R. The line connecting O1 and B intersects the circle with B as the center and R as the radius at point Q2. The trajectory is to spiral from A' with B as the center and R as the radius to point Q2, and then from point Q2 with O1 as the center and R as the radius to point Q1. Q1 is the current planned endpoint. The subsequent trajectory is to start from Q1 and spiral along BC to C.

[0040] refer to Figure 28 The 2nd transition mode of hovering midpoint is as follows: Determine point B and the endpoint A' of the hovering endpoint mode. Determine Q1 on a circle with B as the center and R as the radius. Q1 satisfies that Q1C is perpendicular to Q1B. The trajectory is to hover from A' with B as the center and R as the radius to point Q1. Q1 is the currently planned endpoint. The subsequent trajectory is to start from Q1 and reach C along Q1C.

[0041] refer to Figure 29 and Figure 30 The third transition mode at the midpoint of the hovering point is: when C and B hover in the same direction, refer to... Figure 29 Determine point B and the endpoint A' of the hovering endpoint mode. Determine Q1 on a circle with B as the center and R as the radius, and determine Q2 on a circle with C as the center and R as the radius. Q1Q2=BC and Q1Q2 is parallel to BC. The trajectory is to hover from A' with B as the center and R as the radius to point Q1. Q1 is the currently planned endpoint. The subsequent trajectory is to start from Q1 and go along Q1Q2 to Q2, and then start from Q2 and hover clockwise with C as the center and R as the radius. When C and B rotate in different directions, refer to Figure 30 Determine point B and the endpoint A' of the hovering endpoint mode. Determine the midpoint Q3 of BC. Determine Q1 on a circle with B as the center and R as the radius. Determine Q2 on a circle with C as the center and R as the radius. Q1Q3 is perpendicular to Q1B, and Q2Q3 is perpendicular to Q2C. The trajectory is to hover from A' with B as the center and R as the radius to point Q1. Q1 is the currently planned endpoint. The subsequent trajectory is to start from Q1, go along Q1Q2 to Q2, and then start from Q2 to hover counterclockwise with C as the center and R as the radius.

[0042] refer to Figure 31The 4th transition mode of the hovering midpoint is as follows: Determine point B and the endpoint A' of the hovering endpoint mode. Determine Q1 on a circle with B as the center and R as the radius. Q1 is located on the extension of BC, and BQ1=R. Then determine the center O1, BO1=R, and BO1 is perpendicular to BQ1. Determine Q2 on a circle with O1 as the center and R as the radius. Q1Q2 is parallel to BO1 and Q1Q2=R. The trajectory is to hover from A' with B as the center and R as the radius, hover to Q1, cut out and reach Q2 along Q1Q2. Then, starting from Q2, hover from O1 as the center and R as the radius to B. B is the currently planned endpoint. The subsequent trajectory is to start from B and reach C along BC.

[0043] refer to Figure 32 The 5th transition mode of the hovering midpoint is as follows: Determine point B and the endpoint A' of the hovering endpoint mode. The line connecting BC intersects the circle with B as the center and R as the radius at Q1. Determine O1 on the extension of BC, O1Q1=R. Determine Q2 on the circle with C as the center and R as the radius, Q2C is perpendicular to BC. Determine Q3 on the circle with O1 as the center and R as the radius, Q3O1 is perpendicular to BO1. The trajectory is to start from A' and hover clockwise with B as the center and R as the radius, hover to Q1, and then hover counterclockwise with O1 as the center and R as the radius to Q3. Q3 is the currently planned endpoint. The subsequent trajectory is to start from Q3 and hover along Q3Q2 to Q2, and then hover counterclockwise with C as the center and R as the radius starting from Q2.

[0044] Furthermore, given a turning radius of R, define A as the current waypoint, B as the next waypoint, and C as the next waypoint after B; (Refer to...) Figure 20 The ordinary midpoint pattern 1 is as follows: Determine the center of the incircle of ∠ABC with radius R as O. The incircle is tangent to AB at Q1 and tangent to BC at Q2. The trajectory starts from A and moves along AQ1 to Q1. Starting from Q1, it moves around with center O and radius R to Q2. Q2 is the current planned endpoint. The subsequent trajectory starts from Q2 and moves along Q2C to C.

[0045] refer to Figure 21The second common midpoint pattern is as follows: Determine point O on the median of ∠ABC, where OB=R. Then determine N, where NO is perpendicular to AB and intersects AB at M, where MN=R. Determine O1 on the side of AB away from C, and Q1 on MA, satisfying Q1O1 perpendicular to AB, Q1O1=R, and OO1=2R. Then determine the midpoint Q3 of OO1. Next, determine O2 and Q4 on the side of BC away from A, and Q2 on BC. Q2 and Q1... Symmetrical about the median of ∠ABC, O2 and O1 are symmetrical about the median of ∠ABC, Q4 and Q3 are symmetrical about the median of ∠ABC. The trajectory starts from A and moves along AQ1 to Q1. From Q1, it spirals around O1 with radius R to Q3. From Q3, it spirals around O with radius R to Q4. From Q4, it spirals around O2 with radius R to Q2. Q2 is the current planned endpoint. The subsequent trajectory starts from Q2 and moves along Q2C to C.

[0046] refer to Figure 22 The common midpoint pattern 3 is as follows: Determine point O on the extension of the median of ∠ABC, where OB=R. Determine O1 on the side of AB away from C, and determine Q1 on AB, where BQ1=BC, satisfying Q1O1 perpendicular to AB, Q1O1=R, and OO1=2R. Then determine the midpoint Q3 of OO1. Next, determine O2 and Q4 on the side of BC away from A, and determine Q2 on BC. Q2 and Q1 are symmetrical about the median of ∠ABC, and O2 and O1 are symmetrical about ∠ABC. Symmetrical about the median, Q4 and Q3 are symmetrical about the median of ∠ABC. The trajectory starts at A and proceeds along AQ1 to Q1. From Q1, it circles around O1 with radius R to Q3. From Q3, it circles around O with radius R to Q4. From Q4, it circles around O2 with radius R to Q2, where Q2 is the current planned endpoint. The subsequent trajectory starts at Q2 and proceeds along Q2C to C. The second specific condition is that AB > BC and point C lies on the circle centered at O2. BO <R。 Figure 22 In the middle, C and Q2 coincide.

[0047] refer to Figure 23, the ordinary midpoint mode 4 is as follows: Determine O1 on the side of AB far from C, and determine Q1 on AB, where BQ1 = BA, satisfying that Q1O1 is perpendicular to AB, Q1O1 = R, OO1 = 2R. Then determine O2 on the side of BC far from A, and determine Q2 on BC. Q2 is symmetric to Q1 with respect to the median of ∠ABC, and O2 is symmetric to O1 with respect to the median of ∠ABC. Determine circle O. Circle O is tangent to circle O1 and circle O2. The tangent point of circle O and circle O1 is Q3, and the tangent point of circle O and circle O2 is Q4. The trajectory is starting from A along AQ1 to Q1, starting from Q1 and spiraling with O1 as the center and R as the radius to Q3, starting from Q3 and spiraling with O as the center and R as the radius to Q4, starting from Q4 and spiraling with O2 as the center and R as the radius to Q2. Q2 is the currently planned end point, and the subsequent trajectory is starting from Q2 along Q2C to C; The third specific condition is: AB < BC and point A is on the circle with O2 as the center, and BO < R. Figure 23 In this case, A coincides with Q1.

[0048] Reference Figure 24 , the ordinary midpoint mode 5 is as follows: Determine O1 on the side of AB far from C, O1B is perpendicular to AB, and O1B = R. Determine point O on the extension of AB, OO1 = 2R. Then determine the midpoint Q3 of OO1. Determine Q2 on the side of BC far from A. Q2 is on the circle with O as the center and R as the radius, and Q2C is perpendicular to OQ2. The trajectory is starting from A along AB to B, starting from B and spiraling with O1 as the center and R as the radius to Q3, starting from Q3 and spiraling with O as the center and R as the radius to Q2. Q2 is the currently planned end point, and the subsequent trajectory is starting from Q2 along Q2C to C; The fourth specific condition is: OC > R.

[0049] Reference Figure 25 , the ordinary midpoint mode 6 is as follows: Determine O on the side of AB far from C, satisfying that OB is perpendicular to AB and OB = R. Determine Q2 on the side of AB far from C, satisfying that Q2C is perpendicular to CB and Q2 is on the circle with O as the center and R as the radius. The trajectory is starting from A along AB to B, starting from B and spiraling with O as the center and R as the radius to Q2. Q2 is the currently planned end point, and the subsequent trajectory is starting from Q2 along Q2C to C.

[0050] According to Figures 35 to 38 It can be proved that the planned trajectory obtained by the UAV trajectory planning method provided by the present invention can ensure that the actual flight trajectory highly coincides with the planned trajectory, what you see is what you get, and it avoids the problem that the map planned route does not match the actual route.

[0051] It should be understood that the various forms of processes shown above can be used to reorder, add, or delete steps. For example, the steps described in this invention disclosure can be executed in parallel, sequentially, or in different orders, as long as the desired result of the technical solution disclosed in this invention can be achieved, and this is not limited herein.

[0052] The specific embodiments described above do not constitute a limitation on the scope of protection of this invention. Those skilled in the art should understand that various modifications, combinations, sub-combinations, and substitutions can be made according to design requirements and other factors. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of this invention should be included within the scope of protection of this invention.

Claims

1. A method for planning the trajectory of an unmanned aerial vehicle (UAV), characterized in that, Comprising: Step 1, obtaining the starting point of the current planning, judging whether there is still a trajectory after the current waypoint, if yes, executing the next step, if no, ending the flight path planning; Step 2, obtaining the trajectory of this planning, judging the type of the current waypoint, If the current waypoint is the starting point and the starting point is the hovering point, step 3 is executed; Step 3, if the current waypoint is the terminal point, step 3-1 is executed, if the current waypoint is not the terminal point and the next waypoint is the hovering point, step 3-1 is executed, if the current waypoint is not the terminal point and the next waypoint is not the hovering point, step 3-2 is executed; Step 3-1, judging whether the distance between the current waypoint and the next waypoint meets the first preset condition, if yes, executing the hovering starting point mode 2, if no, continuing to judge whether the directions of the front and rear hovering points are the same, if yes, executing the hovering starting point mode 4, if no, judging whether the distance between the current waypoint and the next waypoint meets the second preset condition, if yes, executing the hovering starting point mode 4, if no, executing the hovering starting point mode 6; Step 3-2, judging whether the distance between the current waypoint and the next waypoint meets the third preset condition, if yes, executing the hovering starting point mode 2, if no, continuing to judge whether the distance between the current waypoint and the next waypoint meets the fourth preset condition, if yes, executing the hovering starting point mode 3, if no, executing the hovering starting point mode 5; Wherein, after executing the hovering starting point mode 1, the hovering starting point mode 2, the hovering starting point mode 3, the hovering starting point mode 4, the hovering starting point mode 5 or the hovering starting point mode 6, the terminal point of the current planning is obtained, the terminal point of the current planning is taken as the starting point of the next planning and returns to step 1.

2. The unmanned aerial vehicle trajectory planning method according to claim 1, wherein, After step 2 judges the type of the current waypoint, it further comprises: if the current waypoint is the starting point and the starting point is the ordinary point, continuing to judge the type of the next waypoint, if the next waypoint is the hovering point, executing step 4, if the next waypoint is the ordinary point, adding the ordinary waypoint mark to the current waypoint and the next waypoint; Step 4 comprises: judging whether the distance between the current waypoint and the next waypoint meets the fifth preset condition, if yes, adding the ordinary waypoint mark to the current waypoint and the next waypoint, if no, marking the current waypoint entering the next waypoint in the tangent manner; The first presupposition condition is: AB > 2 × R A ×cos(asin(0.5))+2×R B ×cos(asin(0.5)); The second preset condition is: AB>R A +R B The third presupposition condition is: AB > 2 × R A ×cos(asin(0.5)); The fourth preset condition is: AB>R A The fifth presupposition is: AB > 2 × R B ×cos(asin(0.5)) or AB <R B AB represents the length of the line connecting the current waypoint and the next waypoint, R A R represents the turning radius of the current waypoint. B The turning radius representing the next waypoint; After adding the ordinary waypoint mark to the current waypoint and the next waypoint or entering the next waypoint in the tangent manner, the next waypoint is taken as the terminal point of the current planning, the terminal point of the current planning is taken as the starting point of the next planning and returns to step 1. 3.The method of claim 1, wherein, After step 2 judges the type of the current waypoint, it further comprises: If the current waypoint is the terminal point and the terminal point is the hovering point, step 5 is executed, if the current waypoint is the terminal point and the terminal point is the ordinary point, the ordinary waypoint terminal point mode is executed; Step 5 includes: judging whether the spiral point enters in a tangent manner, if not, continuing to judge whether the distance between the current waypoint and the next waypoint satisfies a sixth preset condition, if yes, executing a spiral endpoint mode 1, if not, executing a spiral endpoint mode 2, if enters in a tangent manner, executing a spiral endpoint mode 3, the sixth preset condition is: AB>2×R×cos(asin(0.5)) B AB represents the length of the line between the current waypoint and the next waypoint, R represents the spiral radius of the next waypoint B ​ After executing the ordinary waypoint terminal point mode, executing the hovering terminal point mode 1, executing the hovering terminal point mode 2 or executing the hovering terminal point mode 3, the terminal point of the current planning is obtained, the terminal point of the current planning is taken as the starting point of the next planning and returns to step 1. 4.The method of claim 1, wherein, After step 2 judges the type of the current waypoint, it further comprises: If the current waypoint is an intermediate point and the intermediate point is a jump point, it is determined whether the current waypoint is a hovering point; if yes, a hovering end point step is executed, after the execution of the hovering end point step, a hovering intermediate point step is executed, and after the execution of the hovering intermediate point step, it is determined whether the jump target point is a hovering point; if yes, the hovering end point step is executed again, after the execution of the hovering end point step, the hovering intermediate point step is executed, and after the execution of the hovering intermediate point step, the current planning is ended to obtain an end point of the current planning and return to step 1; if the target point is not a hovering point, it is determined whether a first specific condition is met; if yes, a jump into mode 1 is executed, and if not, a jump into mode 2 is executed; if the current waypoint is not a hovering point, a normal intermediate point step is executed, and after the execution of the normal intermediate point step, it is determined whether the jump target point is a hovering point; if yes, the hovering end point step is executed again, after the execution of the hovering end point step, the hovering intermediate point step is executed, and after the execution of the hovering intermediate point step, the current planning is ended to obtain an end point of the current planning and return to step 1; if the target point is not a hovering point, it is determined whether the first specific condition is met; if yes, the jump into mode 1 is executed, and if not, the jump into mode 2 is executed; If the current waypoint is an intermediate point and the intermediate point is not a jump point but a hovering point, the hovering end point step is executed, after the execution of the hovering end point step, the hovering intermediate point step is executed, and after the execution of the hovering intermediate point step, the current planning is ended to obtain an end point of the current planning and return to step 1. If the current waypoint is an intermediate point and the intermediate point is not a jump point but a normal point, the normal intermediate point step is executed, and after the execution of the normal intermediate point step, the current planning is ended to obtain an end point of the current planning and return to step 1.

5. The unmanned aerial vehicle trajectory planning method according to claim 4, wherein the hovering end point step comprises: determining whether the hovering point enters in a tangential manner; if not, it is determined whether a distance between the current waypoint and a next waypoint meets a sixth preset condition; if yes, a hovering end point mode 1 is executed, and if not, a hovering end point mode 2 is executed; if the hovering point enters in the tangential manner, a hovering end point mode 3 is executed. ​ The spiral midpoint step comprises: judging whether the next waypoint is a spiral point, if yes, executing step 5, if not, executing step 6; step 6 comprises: judging whether the distance between the current waypoint and the next waypoint satisfies the seventh preset condition, if yes, executing the spiral midpoint transition mode 1, if not, continuing to judge whether the distance between the current waypoint and the next waypoint satisfies the eighth preset condition, if yes, executing the spiral midpoint transition mode 2, if not, executing the spiral midpoint transition mode 4; step 5 comprises: judging whether the distance between the current waypoint and the next waypoint satisfies the ninth preset condition, if yes, executing the spiral midpoint transition mode 1, if not, continuing to judge whether the directions of the spiral points are the same, if yes, executing the spiral midpoint transition mode 3, if not, judging whether the distance between the current waypoint and the next waypoint satisfies the tenth preset condition, if yes, executing the spiral midpoint transition mode 3, if not, executing the spiral midpoint transition mode 5; The sixth preset condition is: AB>2×R B The seventh preset condition is: AB>2×R B The eighth preset condition is: AB>R B The ninth preset condition is: AB>2×2×R B The tenth preset condition is: AB>2×R B AB represents the length of a line connecting the current waypoint and the next waypoint, and R B represents the radius of the next waypoint. 6.The method of claim 4, wherein, The ordinary midpoint step comprises: judging whether the distance between the current waypoint and the next waypoint satisfies the eleventh preset condition, if yes, executing the ordinary midpoint mode 1, if not, continuing to judge whether the distance between the current waypoint and the next waypoint and the distance between the next waypoint and the next next waypoint satisfy the twelfth preset condition, if yes, executing the ordinary midpoint mode 2, if not, continuing to judge whether the second specific condition is satisfied, if yes, executing the ordinary midpoint mode 3, if not, continuing to judge whether the third specific condition is satisfied, if yes, executing the ordinary midpoint mode 4, if not, continuing to judge whether the fourth specific condition is satisfied, if yes, executing the ordinary midpoint mode 5, if not, executing the ordinary midpoint mode 6, The eleventh preset condition is that AB and BC simultaneously satisfy greater than R / (tan(∠ABC / 2)), and ∠ABC is greater than 60°; the twelfth preset condition is that AB and BC simultaneously satisfy greater than 2×R×sin(acos(R×sin(∠ABC / 2))+R) / (2×R)))+R×cos(∠ABC / 2)), AB represents the length of the line segment between the current waypoint and the next waypoint, and BC represents the length of the line segment between the next waypoint and the next next waypoint; After executing the ordinary midpoint mode 1, the ordinary midpoint mode 2, the ordinary midpoint mode 3, the ordinary midpoint mode 4, the ordinary midpoint mode 5 or the ordinary midpoint mode 6, a current planned terminal point is obtained, and the current planned terminal point is taken as a starting point of next planning and returns to step 1. 7.The method of claim 2, wherein, It is known that the spiral radius is R, A is defined as the current waypoint, and B is defined as the next waypoint; The spiral starting point mode 1 is that the trajectory is spiral with A as the center and R as the spiral radius. The spiral starting point mode 2 is: when B is the spiral point, the line AB between A and B is known, points Q1 and Q21 located on AB are determined, AQ1 = 2 * R * cos (asin (0.5)), BQ21 = 2 * R * cos (asin (0.5)), the perpendicular line O1Q1 of AB is determined, O1Q1 = R, the line between O1 and A intersects with the circle with A as the center and R as the radius at the point Q2, the perpendicular line O21Q21 of AB is determined, O21Q21 = R, the line between O21 and B intersects with the circle with B as the center and R as the radius at the point Q3, the trajectory is spiraled with A as the center and R as the spiral radius, after spiraling to the point Q2, it is spiraled to the point Q1 with O1 as the center and R as the radius, Q1 is the current planning endpoint, and the subsequent trajectory is from Q1 to Q21 along AB, and then spiraled to Q3 with O21 as the center and R as the radius; When B is a normal waypoint, the line AB between A and B is known, a point Q1 located on AB is determined, AQ1 = 2 * R * cos (asin (0.5)), the perpendicular line O1Q1 of AB is determined, O1Q1 = R, the line between O1 and A intersects with the circle with A as the center and R as the radius at the point Q3, the trajectory is spiraled with A as the center and R as the spiral radius, after spiraling to the point Q3, it is spiraled to the point Q1 with O1 as the center and R as the radius, Q1 is the current planning endpoint, and the subsequent trajectory is from Q1 to B along AB; The spiral starting point mode 3 is: Q1 is determined on the circle with A as the center and R as the radius, Q1 satisfies Q1B perpendicular to Q1A, the trajectory is spiraled to the point Q1 with A as the center and R as the radius, Q1 is the current planning endpoint, and the subsequent trajectory is from Q1 to B along Q1B; The spiral starting point mode 4 is: when B and A have the same spiral direction, Q1 is determined on the circle with A as the center and R as the radius, Q2 is determined on the circle with B as the center and R as the radius, Q1Q2 = AB, and Q1Q2 is parallel to AB, the trajectory is spiraled to the point Q1 with A as the center and R as the radius, Q1 is the current planning endpoint, and the subsequent trajectory is from Q1 to Q2 along Q1Q2, and then spiraled to Q2 with B as the center and R as the radius; When B and A have different spiral directions, the midpoint Q3 of AB is determined, Q1 is determined on the circle with A as the center and R as the radius, Q2 is determined on the circle with B as the center and R as the radius, Q1Q3 is perpendicular to Q1A, and Q2Q3 is perpendicular to Q2B, the trajectory is spiraled to the point Q1 with A as the center and R as the radius, Q1 is the current planning endpoint, and the subsequent trajectory is from Q1 to Q2 along Q1Q2, and then spiraled to Q2 with B as the center and R as the radius. The spiral starting point mode 5 is that Q1 and O1 are determined on a circle with A as the center and R as the radius, Q1 is located on the extension line of AB, AO1=R, and AO1 is perpendicular to AQ1, Q2 is determined on a circle with O1 as the center and R as the radius, Q1Q2 is perpendicular to AQ1, Q1Q2=R, the trajectory is spiraling with A as the center and R as the radius, spiraling to Q1 and cutting out and reaching Q2 along Q1Q2, and then spiraling with O1 as the center and R as the radius from Q2, and after spiraling to A, the position of A is taken as the current planning endpoint, and the subsequent trajectory is from A to B along AB; The spiral starting point mode 6 is that AB intersects with a circle with A as the center and R as the radius at Q1, O1 is determined on the extension line of AB, O1Q1=R, Q2 is determined on a circle with B as the center and R as the radius, Q2B is perpendicular to AB, Q3 is determined on a circle with O1 as the center and R as the radius, Q3Q2 is perpendicular to Q2B, the trajectory is spiraling with A as the center and R as the radius, spiraling to Q1 with O1 as the center and R as the radius to Q3, Q3 is the current planning endpoint, and the subsequent trajectory is from Q3 to Q2 along Q3Q2, and then spiraling with B as the center and R as the radius from Q2. 8.The method of claim 3, wherein, It is known that the spiral radius is R, and A is defined as the current waypoint, and B is the next waypoint; The general waypoint endpoint mode is that the trajectory is from A to B along AB, and B is the current planning endpoint; The spiral endpoint mode 1 is that a point Q1 located on AB is determined, which satisfies BQ1=2×R×cos(asin(0.5)), a perpendicular O1Q1 of AB is determined, O1Q1=R, the line connecting O1 and B intersects with a circle with B as the center and R as the radius at Q3 point, the trajectory is from A to Q1 along AQ1, then spirals from Q1 to Q3 with O1 as the center and R as the radius, and then spirals from Q3 with B as the center and R as the spiral radius; The spiral endpoint mode 2 is that Q1 and O1 are determined on a circle with B as the center and R as the radius, Q1 is located on the extension line of BA, BO1=R, and BO1 is perpendicular to BQ1, Q2 is determined on a circle with O1 as the center and R as the radius, Q1Q2=R, and Q1Q2 is perpendicular to Q1B, the trajectory is from A to B along AB, from B to Q2 spiraling with O1 as the center and R as the radius, cutting out from Q2 and reaching Q1 along Q2Q1, and then spiraling from Q1 with B as the center and R as the radius; The spiral endpoint mode 3 is that Q1 is determined on a circle with B as the center and R as the radius, Q1 satisfies Q1B is perpendicular to Q1A, the trajectory is from A to Q1 along AQ1, and then spirals from Q1 with B as the center and R as the radius. 9.The method of claim 5, wherein, It is known that the spiral radius is R, and A is defined as the current waypoint, B is the next waypoint, and C is the next waypoint of B. The jump-in mode 1 is: determining O on the side of BC facing A, satisfying that OB is perpendicular to BC and OB=R, determining O1 on the side of AB away from C, and determining Q1 on AB, satisfying that Q1O1 is perpendicular to AB and Q1O1=R, OO1=2×R, obtaining the midpoint Q3 of Q1O, and the trajectory is from A to Q1 along AB, from Q1 to Q3 by circling with O1 as the center and R as the radius, and from Q3 to B by circling with O as the center and R as the radius, B being the current planned endpoint, and the subsequent trajectory being from B to C along BC; the first specific condition is that Q1 is between AB; The jump-in mode 2 is: determining O1 on the side of AB away from C, satisfying that O1B is perpendicular to AB and O1B=R, determining O on the side of BC away from A, satisfying that OB is perpendicular to BC and OB=R, determining Q1 on the circle with O1 as the center and R as the radius, and determining Q2 on the circle with O as the center and R as the radius, satisfying that Q1Q2 is parallel to OO1, the trajectory is from A to B along AB, from B to Q1 by circling with O1 as the center and R as the radius, from Q1 to Q2 along Q1Q2, and from Q2 to B by circling with O as the center and R as the radius, B being the current planned endpoint, and the subsequent trajectory being from B to C along BC; The circling midpoint transition mode 1 is: when C is a circling point, determining B and the endpoint A' of the circling endpoint mode, knowing the line BC between B and C, determining points Q1 and Q21 on BC, satisfying that BQ1=2×R×cos(asin(0.5)) and CQ21=2×R×cos(asin(0.5)), determining the perpendicular line O1Q1 of BC, O1Q1=R, the line between O1 and B intersecting the circle with B as the center and R as the radius at Q2, determining the perpendicular line O21Q21 of BC, O21Q21=R, the line between O21 and C intersecting the circle with C as the center and R as the radius at Q3, the trajectory being from A' to Q2 by circling with B as the center and R as the radius, from Q2 to Q1 by circling with O1 as the center and R as the radius, Q1 being the current planned endpoint, and the subsequent trajectory being from Q1 to Q21 along BC, and from Q21 to Q3 by circling with O21 as the center and R as the radius, and circling with C as the center and R as the radius from Q3; When C is a common waypoint, determining B and the endpoint A' of the circling endpoint mode, knowing the line BC between B and C, determining point Q1 on BC, satisfying that BQ1=cos(asin(0.5)), determining the perpendicular line O1Q1 of BC, O1Q1=R, the line between O1 and B intersecting the circle with B as the center and R as the radius at Q2, the trajectory being from A' to Q2 by circling with B as the center and R as the radius, from Q2 to Q1 by circling with O1 as the center and R as the radius, Q1 being the current planned endpoint, and the subsequent trajectory being from Q1 to C along BC. The spiral midpoint transition mode 2 is: determining the point B and the end point A' of the spiral end point mode, determining Q1 on the circle with B as the center and R as the radius, Q1 satisfies Q1C is perpendicular to Q1B, the trajectory is from A' to spiral with B as the center and R as the radius to Q1, Q1 is the current planned end point, and the subsequent trajectory is from Q1 to C along Q1C; The spiral midpoint transition mode 3 is: when the spiral direction of C is the same as that of B, determining the point B and the end point A' of the spiral end point mode, determining Q1 on the circle with B as the center and R as the radius, determining Q2 on the circle with C as the center and R as the radius, Q1Q2=BC, and Q1Q2 is parallel to BC, the trajectory is from A' to spiral with B as the center and R as the radius to Q1, Q1 is the current planned end point, and the subsequent trajectory is from Q1 to Q2 along Q1Q2, and then from Q2 to spiral with C as the center and R as the radius; When the spiral direction of C is different from that of B, determining the point B and the end point A' of the spiral end point mode, determining the midpoint Q3 of BC, determining Q1 on the circle with B as the center and R as the radius, and determining Q2 on the circle with C as the center and R as the radius, Q1Q3 is perpendicular to Q1B, Q2Q3 is perpendicular to Q2C, the trajectory is from A' to spiral with B as the center and R as the radius to Q1, Q1 is the current planned end point, and the subsequent trajectory is from Q1 to Q2 along Q1Q2, and then from Q2 to spiral with C as the center and R as the radius; The spiral midpoint transition mode 4 is: determining the point B and the end point A' of the spiral end point mode, determining Q1 and O1 on the circle with B as the center and R as the radius, Q1 is located on the extension line of BC, BQ1=R, and then determining the center O1, BO1=R, and BO1 is perpendicular to BQ1, determining Q2 on the circle with O1 as the center and R as the radius, Q1Q2 is parallel to BO1 and Q1Q2=R, the trajectory is from A' to spiral with B as the center and R as the radius, cuts out Q1 and reaches Q2 along Q1Q2 after spiraling to Q1, and then from Q2 to spiral with O1 as the center and R as the radius to B, B is the current planned end point, and the subsequent trajectory is from B to C along BC; The spiral midpoint transition mode 5 is: determining the point B and the end point A' of the spiral end point mode, BC intersects the circle with B as the center and R as the radius at Q1, determining O1 on the extension line of BC, O1Q1=R, determining Q2 on the circle with C as the center and R as the radius, Q2C is perpendicular to BC, determining Q3 on the circle with O1 as the center and R as the radius, Q3O1 is perpendicular to BO1, the trajectory is from A' to spiral with B as the center and R as the radius, spirals to Q1, and then spirals to Q3 with O1 as the center and R as the radius, Q3 is the current planned end point, and the subsequent trajectory is from Q3 to Q2 along Q3Q2, and then from Q2 to spiral with C as the center and R as the radius. 10.The method of claim 6, wherein, It is known that the spiral radius is R, A is the current waypoint, B is the next waypoint, and C is the next waypoint of B. The common midpoint mode 1 is: determining the center O of the incircle of ∠ABC with R as the radius, the incircle is tangent to AB at Q1, the incircle is tangent to BC at Q2, the trajectory is from A along AQ1 to Q1, from Q1 spirals to Q2 with O as the center and R as the radius, Q2 is the current planned endpoint, and the subsequent trajectory is from Q2 along Q2C to C; The common midpoint mode 2 is: determining the point O on the median of ∠ABC, OB=R, then determining N, NO is perpendicular to AB, NO intersects AB at M, MN=R, then determining O1 on the side of AB away from C, and determining Q1 on MA, which satisfies that Q1O1 is perpendicular to AB, Q1O1=R, OO1=2R, then determining the midpoint Q3 of OO1, then determining O2 and Q4 on the side of BC away from A, and determining Q2 on BC, Q2 is symmetric to Q1 about the median of ∠ABC, O2 is symmetric to O1 about the median of ∠ABC, Q4 is symmetric to Q3 about the median of ∠ABC, the trajectory is that A starts along AQ1 to Q1, from Q1 spirals to Q3 with O1 as the center and R as the radius, from Q3 spirals to Q4 with O as the center and R as the radius, from Q4 spirals to Q2 with O2 as the center and R as the radius, Q2 is the current planned endpoint, and the subsequent trajectory is from Q2 along Q2C to C; The common midpoint mode 3 is: determining the point O on the extension of the median of ∠ABC, OB=R, then determining O1 on the side of AB away from C, and determining Q1 on AB, BQ1=BC, which satisfies that Q1O1 is perpendicular to AB, Q1O1=R, OO1=2R, then determining the midpoint Q3 of OO1, then determining O2 and Q4 on the side of BC away from A, and determining Q2 on BC, Q2 is symmetric to Q1 about the median of ∠ABC, O2 is symmetric to O1 about the median of ∠ABC, Q4 is symmetric to Q3 about the median of ∠ABC, the trajectory is that A starts along AQ1 to Q1, from Q1 spirals to Q3 with O1 as the center and R as the radius, from Q3 spirals to Q4 with O as the center and R as the radius, from Q4 spirals to Q2 with O2 as the center and R as the radius, Q2 is the current planned endpoint, and the subsequent trajectory is from Q2 along Q2C to C; the second specific condition is that AB>BC and the point C is located on the circle with O2 as the center, BO<R; The common midpoint mode 4 is: determining O1 on the side of AB far away from C, and determining Q1 on AB, BQ1=BA, Q1O1 is perpendicular to AB, Q1O1=R, OO1=2R, determining O2 on the side of BC far away from A, and determining Q2 on BC, Q2 is symmetric to Q1 about the angle bisector of ABC, O2 is symmetric to O1 about the angle bisector of ABC, determining circle O, circle O is tangent to circle O1 and circle O2, the tangent point of circle O and circle O1 is Q3, the tangent point of circle O and circle O2 is Q4, the trajectory is that A starts to reach Q1 along AQ1, Q1 starts to spiral to Q3 with O1 as the center and R as the radius, Q3 starts to spiral to Q4 with O as the center and R as the radius, Q4 starts to spiral to Q2 with O2 as the center and R as the radius, Q2 is the current planned terminal point, and the subsequent trajectory is that Q2 starts to reach C along Q2C; the third specific condition is that AB<BC and A is located on a circle with O2 as the center, BO<R; The common midpoint mode 5 is: determining O1 on the side of AB far away from C, O1B is perpendicular to AB, and O1B=R, determining O on the extension line of AB, OO1=2R, determining the midpoint Q3 of OO1, determining Q2 on the side of BC far away from A, Q2 is located on a circle with O as the center and R as the radius, and Q2C is perpendicular to OQ2, the trajectory is that A starts to reach B along AB, B starts to spiral to Q3 with O1 as the center and R as the radius, Q3 starts to spiral to Q2 with O as the center and R as the radius, Q2 is the current planned terminal point, and the subsequent trajectory is that Q2 starts to reach C along Q2C; the fourth specific condition is that OC>R; The common midpoint mode 6 is: determining O on the side of AB far away from C, OB is perpendicular to AB, and OB=R, determining Q2 on the side of AB far away from C, Q2C is perpendicular to CB, and Q2 is located on a circle with O as the center and R as the radius, the trajectory is that A starts to reach B along AB, B starts to spiral to Q2 with O as the center and R as the radius, Q2 is the current planned terminal point, and the subsequent trajectory is that Q2 starts to reach C along Q2C.

Images