Route planning method and device based on no-fly zone constraintTechnical Field
The invention relates to the field of algorithm design and simulation, in particular to a method and a device for planning an air route based on restricted flight zones.
Background
Unmanned aerial vehicle can fly at high, low latitude to its characteristics nimble, convenient can accomplish specific tasks such as spraying insecticide, safety monitoring. In the flight process of completing the task, the unmanned aerial vehicle can meet the areas such as a high building, a high-voltage wire and a safe no-fly area, and at the moment, the unmanned aerial vehicle needs to bypass the no-fly area to fly to the area of the task to be executed. In addition, in the flight process of completing a specific task, the unmanned aerial vehicle has limited loaded energy and limited flight time, so that the unmanned aerial vehicle needs to complete the specific task in limited time, a feasible air route needs to be solved before flight, and an optimal air route is selected for flight; meanwhile, when meeting an unplanned no-fly area in the flying process, the feasible air route can be solved in real time and the optimal air route is selected for flying.
In the prior art, an important way for solving a feasible air route of an unmanned aerial vehicle is to adopt an air route planning algorithm, at present, the air route planning algorithm is many, and common algorithms comprise a geometric method and an intelligent planning algorithm based on a graph theory.
Disclosure of Invention
The inventor finds in the research process that the existing geometric method based on graph theory requires that all possible routes in the corresponding space must be listed in a graph model used for representing a planning space or a state space, otherwise, the method may lose the optimal solution. In addition, the calculation process of the intelligent air route planning algorithm is time-consuming generally, so that when the unmanned aerial vehicle encounters a no-fly area outside the plan, the optimal air route cannot be solved in real time.
In view of the above, the main object of the present invention is to provide a method and an apparatus for planning an airway based on forbidden flight zone constraint, which can not only solve the optimal airway in the planned space before the flight of the unmanned aerial vehicle, but also solve the optimal airway in real time when the unmanned aerial vehicle encounters a forbidden flight zone outside the planning, and do not lose the optimal solution in the process of solving the optimal airway.
In order to solve the problems, the invention discloses a method for planning an air route based on restricted flight zones, which is applied to an unmanned aerial vehicle and comprises the following steps:
the unmanned aerial vehicle is segmented into a crossing area and a dense small area in a no-fly area from a starting position to an end position to obtain different segmented no-fly areas;
determining an area contained by an circumscribed circle of each sliced flight-forbidden area as a circular flight-forbidden area;
solving each straight feasible path and feasible route point between the starting point position and the end point position by adopting a method of inner and outer common tangents of a circle between the circular no-fly areas, wherein the feasible route point is an intersection point of the straight feasible path and the circular no-fly areas;
determining all feasible routes from the starting position to the end position according to the straight feasible path;
and planning an optimal route from the starting position to the end position according to all feasible routes and the feasible route points.
Wherein the planning the optimal route from the starting position to the ending position according to all feasible routes and the feasible route points comprises:
selecting the air route with the shortest path from all the feasible air routes as an optimal air route;
and determining a flight mode between two adjacent feasible route points in two adjacent straight-line feasible paths in the optimal route according to whether the attribute of the feasible route point is an entry tangent point or an exit tangent point, so as to obtain the optimal route from the starting point position to the end point position.
Wherein, the determining the flight mode between two adjacent feasible waypoints in two adjacent straight-line feasible paths in the optimal route according to whether the attribute of the feasible waypoint is an entry tangent point or an exit tangent point comprises:
if the two adjacent feasible routes in the two adjacent straight feasible paths in the optimal route are from an exit tangent point to an entry tangent point, adopting a straight flight mode, wherein the exit tangent point is a tangent point of a straight feasible path leaving the circular no-fly area according to the direction of the feasible route and the circular no-fly area, and the entry tangent point is a tangent point of a straight feasible path flying into the circular no-fly area according to the direction of the feasible route and the circular no-fly area;
and if the two adjacent feasible route points in the two adjacent straight feasible paths in the optimal route are from the entry tangent point to the exit tangent point, adopting an arc flight mode.
The method for solving each straight line feasible path and feasible route point between the starting point position and the end point position by adopting the inner and outer common tangent lines of the circle between the circular no-fly areas comprises the following steps:
acquiring the starting position as a current position;
aiming at the current position, executing a searching process of a current straight line feasible path, wherein the searching process comprises the following steps:
determining a current connecting line between the current position and the end position;
judging whether the current connecting line passes through the circular no-fly area;
if the current connecting line passes through the circular no-fly area, determining a circular no-fly area which is closest to the current position on the current connecting line, and determining a current common tangent line which passes through the current position side between the circular no-fly area where the current position is located and the circular no-fly area which is closest to the current position;
judging whether the current common tangent line passes through the circular no-fly area;
if not, judging whether a straight line feasible path adjacent to the current common tangent line is intersected with the path determined by the current common tangent line; if not, determining that the current common tangent line is a current straight line feasible path;
and executing the searching process by taking the end point of the current straight-line feasible path as the current position until all straight-line feasible paths between the starting position and the end point position are determined.
Wherein, the judging whether the straight line feasible path adjacent to the current common tangent line intersects with the path determined by the current common tangent line further comprises:
and if the current connection line is intersected with the circular no-fly area, taking an external common tangent line on the side of the current common tangent line between the circular no-fly area where the starting point of the straight line feasible path adjacent to the current common tangent line is located and the circular no-fly area where the end point of the path determined by the current common tangent line is located as the current connection line, and executing the step of judging whether the current connection line passes through the circular no-fly area.
Wherein, aiming at the current position, a searching process of the current straight line feasible path is executed, and the searching process further comprises:
if the current connecting line does not pass through the circular no-fly area, judging whether the current position is the end point position, and if so, determining that the current connecting line is the current straight line feasible path;
if the current position is not the end position, calculating a current common tangent from the end position to the current position side of the circular no-fly area where the current position is located;
judging whether the current common tangent line passes through the circular no-fly area;
if the current common tangent line passes through the circular no-fly area, taking the current common tangent line as a current connecting line, and executing the step that the current connecting line passes through the circular no-fly area;
and if not, determining that the current connecting line is the current straight-line feasible path.
The invention also provides a route planning device based on no-fly zone restriction, which is integrated on an unmanned aerial vehicle and is characterized by comprising:
the acquisition unit is used for segmenting a cross area and a dense small area in a no-fly area between a starting position and an end position of the unmanned aerial vehicle to obtain different segmented no-fly areas;
a first determining unit, configured to determine, as a circular no-fly area, an area included in an circumscribed circle of each slice no-fly area;
the calculation unit is used for solving each straight feasible path and feasible route point between the starting point position and the end point position by adopting a method of inner and outer common tangents of a circle between the circular no-fly areas, wherein the feasible route point is an intersection point of the straight feasible path and the circular no-fly areas;
the second determining unit is used for determining all feasible routes from the starting position to the end position according to the straight feasible path;
and the third determining unit is used for planning the optimal route from the starting position to the end position according to all feasible routes and the feasible route points.
The third determination unit includes:
and the first determining subunit is used for selecting the air route with the shortest path from all the feasible air routes as the optimal air route.
And the second determining subunit is configured to determine, according to whether the attribute of the feasible route point is an entry tangent point or an exit tangent point, a flight mode between two adjacent feasible route points in two adjacent straight-line feasible paths in the optimal route, so as to obtain an optimal route from the starting point position to the end point position.
Wherein the second determining subunit is specifically configured to:
determining to adopt a linear flight mode under the condition that two adjacent feasible route points in two adjacent linear feasible paths in the optimal route are from an exit tangent point to an entry tangent point; or, if two adjacent feasible route points in two adjacent straight feasible paths in the optimal route are from an entry tangent point to an exit tangent point, an arc flight mode is adopted, the exit tangent point is a tangent point of a straight feasible path leaving the circular no-fly area according to the direction of the feasible route and the circular no-fly area, and the entry tangent point is a tangent point of a straight feasible path flying into the circular no-fly area according to the direction of the feasible route and the circular no-fly area.
Wherein the calculation unit includes:
and the acquisition subunit is used for acquiring the starting point position as the current position.
An execution subunit, configured to determine, for the current position, a current connection line between the current position and the end point position;
judging whether the current connecting line passes through the circular no-fly area;
if the current connecting line passes through the circular no-fly area, determining a circular no-fly area which is closest to the current position on the current connecting line, and determining a current common tangent line which passes through the current position side between the circular no-fly area where the current position is located and the circular no-fly area which is closest to the current position;
judging whether the current common tangent line passes through the circular no-fly area;
if not, judging whether a straight line feasible path adjacent to the current common tangent line is intersected with the path determined by the current common tangent line; if not, determining that the current common tangent is the current straight-line feasible path.
And the triggering subunit is used for searching the straight-line feasible path starting from the current position by taking the end point of the current straight-line feasible path as the current position until all the straight-line feasible paths between the starting point position and the end point position are determined.
Wherein the execution subunit is further configured to:
if the current connecting line does not pass through the circular no-fly area, judging whether the current position is the end point position, and if so, determining that the current connecting line is the current straight line feasible path;
if the current position is not the end position, calculating a current common tangent from the end position to the current position side of the circular no-fly area where the current position is located;
judging whether the current common tangent line passes through the circular no-fly area;
if the current common tangent line passes through the circular no-fly area, taking the current common tangent line as a current connecting line, and executing the step that the current connecting line passes through the circular no-fly area;
and if not, determining that the current connecting line is the current straight-line feasible path.
Wherein, the determining whether the straight feasible path adjacent to the current common tangent intersects with the path determined by the current common tangent may further include:
and if the current connection line is intersected with the circular no-fly area, taking an external common tangent line on the side of the current common tangent line between the circular no-fly area where the starting point of the straight line feasible path adjacent to the current common tangent line is located and the circular no-fly area where the end point of the path determined by the current common tangent line is located as the current connection line, and executing the step of judging whether the current connection line passes through the circular no-fly area.
Compared with the prior art, the invention has the following beneficial effects:
in order to solve the optimal air route before the unmanned aerial vehicle flies, solve the optimal air route in real time when the unmanned aerial vehicle encounters a no-fly area outside the plan in the flying process and avoid losing the optimal solution, the invention provides an air route planning method and device based on the no-fly area constraint. In the process of solving the optimal air route, the no-fly area in the planning space is segmented and integrated into the circumscribed circle area, and the feasible air route and the air route point are solved for the circumscribed circle area by adopting a common tangent method, so that the idea of solving the optimal air route is simplified, the optimal air route is not easy to lose, the solving speed is higher, meanwhile, the selected optimal air route is suitable for the flight mode of the unmanned aerial vehicle and is the shortest air route, the actual flight time of the unmanned aerial vehicle is shortened, and the defects that the optimal solution can be lost by a geometric algorithm based on graph theory and the time consumption of an intelligent air route planning algorithm in the prior art are overcome, so that the unmanned aerial vehicle is ensured to complete a specific task within limited time.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, it is obvious that the drawings in the following description are only embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to the provided drawings without creative efforts.
FIG. 1 is a flowchart of a method for planning an airway based on no-fly zone constraints according to the present invention;
FIG. 2(a) is a schematic diagram of a circular no-fly zone according to the present invention;
fig. 2(b) is a schematic diagram of a circular no-fly area obtained from a dense small area in the present invention;
FIG. 3 is a flowchart of a method for solving all straight-line feasible paths from a starting position to an ending position according to the present invention;
FIG. 4(a) is a schematic diagram of the present invention in which two straight paths exist around the fly-away path;
FIG. 4(b) is a diagram illustrating a new path computed when a fly-around path exists in the present invention;
fig. 5 is a flowchart of a method for planning a no-fly area to obtain a circular no-fly area according to the present invention;
FIG. 6 is a flowchart of a method for finding all straight feasible paths from a starting position to an ending position according to the present invention;
fig. 7 is a schematic structural diagram of an airway planning device based on no-fly zone restriction in the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
When the unmanned aerial vehicle needs to fly from the starting position to the end position to complete a specific task, the flight area where the unmanned aerial vehicle flies from the starting position to the end position is a three-dimensional space area, and in order to plan the route of the unmanned aerial vehicle in the three-dimensional space, the space through which the unmanned aerial vehicle may pass is usually simplified, for example, the three-dimensional space is simplified into a planar space, and the no-fly area is simplified into a simple geometric shape.
When planning a route from a starting position to an end position of an unmanned aerial vehicle, simplifying a three-dimensional space area which the unmanned aerial vehicle may pass through into a plane area, segmenting the no-fly area according to whether the no-fly area in the three-dimensional space has a cross condition and the intensity of the no-fly area to obtain segmented no-fly areas, and determining an area contained by an circumscribed circle of each no-fly area as a circular no-fly area; then, solving each straight line feasible path and feasible route point from the starting point position to the end point position by adopting a method of inner and outer common tangent lines of a circle between the circular no-fly areas; and finally, determining a feasible air route from the starting position to the end position according to each straight feasible path, determining the air route with the shortest path in the feasible air routes as an optimal air route, determining a flight mode between two adjacent straight feasible paths according to the attribute of the feasible air route point of the optimal air route, and further obtaining the optimal air route from the starting position to the end position.
In order that those skilled in the art will better understand the disclosure, the invention will be described in further detail with reference to the accompanying drawings and specific embodiments.
Example 1
Fig. 1 is a flowchart of a method for planning an airway according to embodiment 1 of the present invention, where the method includes:
step S101: and segmenting a crossing area and a dense small area of the unmanned aerial vehicle in the no-fly area from the starting position to the end position to obtain different segmented no-fly areas.
In general, the flight area of the drone from the starting position to the ending position is a three-dimensional area determined in three directions, north, east and sky, wherein the starting position is the origin of the three-dimensional coordinate system. The three-dimensional space area is provided with a plurality of no-fly areas, so that the optimal route from the starting position to the end position of the unmanned aerial vehicle can be conveniently solved, the three-dimensional no-fly areas in the three-dimensional space are projected in planes determined in the north direction and the east direction to obtain the plane no-fly areas, and therefore the problem of solving the optimal route of the unmanned aerial vehicle in the three-dimensional space can be simplified as follows: and solving the optimal route problem between plane no-fly areas in the plane determined in the north and east directions. For convenience of description, the planar no-fly regions that will appear hereinafter are collectively referred to as no-fly regions.
In addition, because a plurality of non-flight areas with irregular shapes and distribution exist in the plane determined in the north direction and the east direction, in order to solve the optimal route of the unmanned aerial vehicle in the plane conveniently, the non-flight areas are divided into pieces according to whether the non-flight areas are cross areas or small dense areas, and the divided non-flight areas are obtained. The intersection area is an area formed by intersecting the no-fly areas, and each intersection area is a sliced no-fly area; the dense small areas are formed in such a way that the distance between adjacent no-fly areas is greater than the minimum turning radius of the unmanned aerial vehicle and less than 2 times of the minimum turning radius of the unmanned aerial vehicle, and each dense small area is also a sliced no-fly area; and determining each no-fly area except for the intersection area and the dense small area in all no-fly areas as one sliced no-fly area.
Step S102: and determining the area contained by the circumscribed circle of each sliced no-fly area as a circular no-fly area.
In order to solve the optimal air route between all the sliced no-fly areas from the starting point position to the end point position conveniently, an circumcircle of each sliced no-fly area is made, the area contained by each obtained circumcircle is determined to be a circular no-fly area, and on the basis of the circular no-fly area, each straight line feasible path and feasible air route point between the starting point position and the end point position are solved by adopting a method of inner and outer common tangents of the circle. For example, for the intersection region, a minimum circumcision circle which can include each intersected no-fly region can be made as a circular no-fly region; for the dense small area, a minimum circumscribed circle including each no-fly area in the dense small area may be made as a circular no-fly area, and for other fragment no-fly areas, the minimum circumscribed circle that can include all areas in the fragment no-fly area may be made as a circular no-fly area. Specifically, reference may be made to fig. 2(a) and fig. 2(b), where fig. 2(a) is a schematic diagram of a circular no-fly region obtained according to an intersection region, and fig. 2(b) is a schematic diagram of a circular no-fly region obtained according to a dense small region.
Step S103: and solving each straight feasible path and feasible route point between the starting point position and the end point position by adopting a method of inner and outer common tangents of a circle between the circular no-fly areas, wherein the feasible route point is an intersection point of the straight feasible path and the circular no-fly area.
In step S103, each straight feasible path and feasible route point from the starting point position to the ending point position is solved by using a method of inner and outer common tangents of a circle between circular no-fly areas. The straight-line feasible path to be solved is a straight-line path between the two circular no-fly areas and does not pass through other circular no-fly areas, and the intersection point of the straight-line feasible path and the circular no-fly areas is a feasible waypoint.
Specifically, the process of solving the straight feasible path and the feasible waypoint is shown in fig. 3, and the process includes steps S301 to S312:
step S301: and acquiring the starting position as the current position.
Firstly, taking the starting point position of the unmanned aerial vehicle to take off as the current position, and finding a straight-line feasible path from the circular no-fly area where the current position is located.
Step S302: for the current position, a current connecting line between the current position and the end position is determined.
After the current position is determined, the current position and the end point position are connected, the straight line connecting the current position and the end point position is determined as the current connection line, and the process advances to step S303.
Step S303: and judging whether the current connecting line passes through the circular no-fly area, if so, entering a step S304, and if not, entering a step S309.
After obtaining the current connecting line between the current position and the end point position, if the flag bit is 1, indicating that the current connecting line passes through other circular no-fly areas from the current position to the end point position, entering step S304 according to whether the flag bit at this time is 1; if the flag bit is not 1, which indicates that the current connection line does not pass through another circular no-fly area from the current position to the end position, the process proceeds to step S309.
It should be noted that the value of the flag bit is used to indicate whether the current connection line passes through other circular no-fly regions, and the flag bit may select any value to indicate whether the current connection line passes through the no-fly region at this time.
Step S304: determining a circular no-fly area closest to the current position on the current connecting line, determining the circular no-fly area where the current position is located and the circular no-fly area closest to the current position, passing through a current common tangent line on the current position side, and entering step S305.
Specifically, after a current connecting line is obtained, firstly, whether the current connecting line passes through a circular no-fly area is judged, and if the current connecting line passes through the circular no-fly area, a circular no-fly area which is closest to the current position on the current connecting line needs to be found; then, a common tangent between the circular no-fly area where the current position is located and the circular no-fly area closest to the current position is obtained, the common tangent includes four common tangents, two of the four common tangents are outer common tangents, the other two common tangents are inner common tangents, two common tangents passing through the side of the circular no-fly area where the current position is located are selected from the four common tangents, one common tangent is selected from the two common tangents as the current common tangent, and step S305 is executed. Until the two common tangents are respectively used as the current common tangent, the step S305 is executed, and at this time, a straight feasible path that starts from the circular no-fly area where the current position is located and does not exist around the fly path is found.
Specifically, when the starting point position is the current position, the area of the circular no-fly area where the current position is located is zero, in this case, the current position and the common tangent of the circular no-fly area closest to the current position are directly obtained, two common tangents are obtained at this time, and the two common tangents are used as the current common tangent until a straight-line feasible path starting from the circular no-fly area where the current position is located is found and no fly-around path exists.
Step S305: and judging whether the current common tangent line passes through a circular flying forbidding area, if so, taking the current common tangent line as a current connecting line, and entering step S304, and if not, entering step S306.
And judging whether the flag bit at the moment is 1, if so, indicating that the current common tangent line passes through other circular no-fly areas, taking the current common tangent line as the current connecting line, executing the step S304, otherwise, indicating that the current common tangent line does not pass through other circular no-fly areas, and executing the step S306.
Specifically, the current common tangent line is used as the current connection line, and according to step S304, the circular no-fly area closest to the current position is continuously searched for, the common tangent line between the circular no-fly area where the current position is located and the circular no-fly area closest to the current position is determined, two common tangent lines on the current position side are selected, one of the two common tangent lines is used as the current common tangent line, step S305 is then executed, and the process is repeated until the current common tangent line does not pass through the circular no-fly area, and step S306 is then performed.
Step S306: judging whether a straight-line feasible path adjacent to the current common tangent line is intersected with a path determined by the current common tangent line; if yes, the process proceeds to step S307, and if no, the process proceeds to step S311.
The purpose of this step is to determine whether a fly-around path exists between the current common tangent line and a straight-line feasible path adjacent to the current common tangent line. Fig. 4(a) is a schematic diagram of a fly-around path formed by intersecting portions of two straight paths, and fig. 4(b) is a new path calculated by the present invention when a fly-around path exists.
Specifically, in fig. 4(a), circle 01, circle 02 and circle 03 respectively represent three circular no-fly areas, and a common tangent p1p2 between circle 01 and circle 02 and a common tangent p3p4 between circle 02 and circle 03 are made, so that an intersection point p1 between the common tangent p1p2 and circle 01 is a feasible waypoint, an intersection point p2 between the common tangent p1p2 and circle 02 is also a feasible waypoint, and p1p2 is a straight feasible path between the circular no-fly area 01 and the circular no-fly area 02. Similarly, p3 and p4 are feasible waypoints, and p3p4 represents a straight feasible path between the circular no-fly area 02 and the circular no-fly area 03. As can be seen from fig. 4(a), the straight feasible path p1p2 and the straight feasible path p3p4 intersect at a point, and if the drone flies from the circular no-fly area 01 to the circular no-fly area 02 and then to the circular no-fly area 03, the flight paths are: firstly, flying along a straight feasible path p1p2, flying to the intersection point of p1p2 and p3p4, then flying to a feasible route point p2, and then flying to a feasible route point p3 by one circle around a circular no-fly area 02; then, flying from the feasible route point p3 to the intersection point of p1p2 and p3p 4; finally, fly from the intersection to the feasible route point p 4. Then the flight from feasible route point p1 to feasible route point p4 is from the intersection of p1p2 and p3p4 to feasible route point p2, then flies one turn around circular no-fly area 02 to feasible route point p3, and the path from feasible route point p3 to the intersection of p1p2 and p3p4 is the fly-around path.
In the algorithm, for judging whether a detour path exists between p3p4 and p1p2, whether a segment p3p4 intersects with the segment p1p2 is adopted, if so, the detour path exists from the point p1 to the point p4, and if not, the detour path does not exist from the point p1 to the point p 4.
Step S307: and step S303 is executed by taking, as a current connection line, an external common tangent line on the current common tangent line side between a circular no-fly area where a start point of a straight-line feasible path adjacent to the current common tangent line is located and a circular no-fly area where an end point of the path determined by the current common tangent line is located.
The step is that there is an action executed under the fly-around path, if there is a fly-around path between the current common tangent and the adjacent straight-line feasible path, the current common tangent needs to be deleted, and a new path needs to be recalculated, and the calculation method of the new path is as follows: firstly, determining a circular no-fly area where a starting point of a straight line feasible path adjacent to a current common tangent is located and a circular no-fly area where an end point of the current common tangent is located; then, an outer common tangent line between the circular no-fly area where the start point of the straight-line feasible path adjacent to the current common tangent line is located and the circular no-fly area where the end point of the current common tangent line is located is made, at this time, two outer common tangent lines are obtained, and the common tangent line on one side of the current common tangent line in the two common tangent lines is determined as the current connecting line, and the process proceeds to step S303.
Specifically, for the detour path from the feasible route point p1 to the feasible route point p4 shown in fig. 4(a), the method for calculating the new path in this step is as follows: deleting the path p3p4, making an external common tangent between the circular no-fly area 01 and the circular no-fly area 03, at this time, obtaining two external common tangents, taking the external common tangent passing through the side where the line segment p1p2 and the line segment p3p4 are located as a current connecting straight line, taking the starting point of the external common tangent as the current position, entering step S303, and obtaining a new path by adopting the method for calculating the new path in the step as shown in fig. 4 (b).
Step S308: and judging whether the current position is the end position, if so, entering step S312, and if not, entering step S309.
In this step, when the determination result in the step S303 is that the current connection line does not pass through the circular no-fly area, it is continuously determined whether the current position is the end position, if so, the process proceeds to a step S312, and if not, the process proceeds to a step S309.
Step S309: and calculating a current common tangent line from the end position to the current position side of the circular no-fly area where the current position is located, and entering step S310.
In this step, when the current connection line between the current position and the end point does not pass through other circular no-fly areas and the current position is not the end point position, at this time, a common tangent line is made from the end point position to the circular no-fly area where the current position is located, because the end point position is a point, there are two common tangent lines, and a common tangent line passing through the current position on one side of the circular no-fly area is selected from the two common tangent lines as the current common tangent line, and then the process proceeds to step S310.
Step S310: and judging whether the current common tangent line passes through a circular flying forbidding area, if so, taking the current common tangent line as a current connecting line, and then entering step S304, and if not, entering step S311.
Step S311: and determining the current common tangent line as a current straight line feasible path, and entering step S312.
In this step, when there is no fly-around path between the current common tangent line and the straight-line feasible path adjacent to the current common tangent line, the current common tangent line obtained by the solving is the straight-line feasible path, and then the process proceeds to step S312, where the end point of the straight-line feasible path is used as the current position to continue to solve the straight-line feasible path.
Step S312: and continuously solving the straight-line feasible path by taking the end point of the straight-line feasible path as the current position until all the straight-line feasible paths between the starting point position and the end point position are determined.
In this step, when it is determined in step S303 that the obtained current connection line between the current position and the end point position does not pass through the circular no-fly area, and the current position is the end point position; or, in step S311, when it is determined that the current common tangent line is the current straight-line feasible path, the step is executed, that is, the straight-line feasible path is found according to steps S302 to S311 until the end point of the current straight-line feasible path is the end point position determined in the embodiment of the present invention, where this indicates that all the straight-line feasible paths from the start point position to the end point position are found completely.
After step S103 is described to solve all the straight-line feasible paths between the start position and the end position, the process then returns to fig. 1 and proceeds to step S104.
Step S104: and determining all feasible routes from the starting position to the end position according to the straight feasible path.
After all the straight-line feasible paths from the starting position to the end position are obtained through solving, when the straight-line feasible paths from the starting position to the end position are obtained along the straight-line feasible paths, a plurality of feasible routes from the starting position to the end position exist. The method for determining the feasible route comprises the following steps: starting from the starting position, along the straight feasible path to the end position, the straight feasible path passed by the process sequence forms a feasible route. And sequentially finding all feasible routes from the starting position to the end position according to the method for finding the feasible routes.
Step S105: and planning an optimal route from the starting position to the end position according to all feasible routes and the feasible route points.
And after all feasible routes between the obtained starting position and the end position are obtained, determining the route with the shortest straight feasible path in all feasible routes as the optimal route. At the moment, the optimal air route comprises a plurality of straight feasible paths selected from the starting position to the end position, and the sequence of the unmanned aerial vehicle flying according to the straight feasible paths.
And then, determining the flight mode of the unmanned aerial vehicle along the optimal route according to the optimal route and the attributes of each feasible route point on the optimal route, and further obtaining the optimal path.
Specifically, if two adjacent feasible route points in two adjacent straight feasible paths in the optimal route are from an exit tangent point to an entry tangent point, the two adjacent feasible route points fly to the entry tangent point along straight lines from the exit tangent point, the exit tangent point is a tangent point between a straight feasible path leaving the circular no-fly area according to the direction of the optimal route and the circular no-fly area, and the entry tangent point is a tangent point between a straight feasible path flying into the circular no-fly area according to the direction of the optimal route and the circular no-fly area.
And if two adjacent feasible route points in two adjacent straight feasible paths in the optimal route are from the entry tangent point to the exit tangent point, flying along an arc line from the entry tangent point to the exit tangent point on the circular no-fly area where the exit tangent point and the entry tangent point are located.
In the embodiment of the invention, firstly, a three-dimensional no-fly area in a three-dimensional space is converted into a no-fly area in a two-dimensional plane; secondly, segmenting a no-fly area in the two-dimensional plane area, wherein the no-fly area with the cross is taken as a segment no-fly area, the distance between two adjacent no-fly areas meets, the no-fly area which is larger than the minimum turning radius of the unmanned aerial vehicle and is smaller than 2 times of the minimum turning radius of the unmanned aerial vehicle is taken as a segment area, and the no-fly area which does not belong to the cross type and does not meet the condition that the distance between the no-fly area and the adjacent no-fly area is larger than the minimum turning radius of the unmanned aerial vehicle and is smaller than 2 times of the minimum turning radius of the unmanned aerial vehicle is taken as a segment no; thirdly, for each fragment no-fly area, a minimum circumscribed circle containing all areas of the fragment no-fly area is taken as a circular no-fly area of the fragment no-fly area, and a method of an inner common tangent line and an outer common tangent line of the circle is adopted to solve a straight-line feasible path between all the circular no-fly areas from a starting point position to an end point position; and finally, determining all feasible routes from the starting position to the end position from all the straight feasible paths, and further determining the optimal route and the optimal airway.
The embodiment of the invention adopts a method of inner and outer common tangents of a circle to solve all straight-line feasible route points between the starting point position and the end point position, then determines feasible routes, and finally determines the optimal routes and the optimal routes. Meanwhile, the defects that the existing intelligent algorithm is time-consuming and is not beneficial to online resolving and engineering application are overcome.
Example 2
The embodiment of the invention describes the implementation process of the route planning method in more detail, and mainly comprises the steps of planning a no-fly area to obtain a circular no-fly area, and searching all straight-line feasible paths from a starting position to an end position based on the determined circular no-fly area.
Specifically, fig. 5 is a flowchart of a method for planning a no-fly area to obtain a circular no-fly area. The method can comprise the following steps:
step S501: a three-dimensional no-fly area which may be encountered by the unmanned aerial vehicle in the space is simplified into a two-dimensional plane no-fly area, and the plane no-fly area is collectively referred to as a no-fly area below.
Step S502: and (4) segmenting the intersection area and the dense small area in the no-fly area to obtain different segmented no-fly areas.
The steps S501 to S502 correspond to the step S101 in the embodiment 1, and for the specific implementation steps of the steps S501 to S502, refer to the step S101 in the embodiment 1, which is not described herein again.
Step S503: and making an circumscribed circle of each fragment no-fly area, and taking an area determined by the circumscribed circle as a circular no-fly area corresponding to the fragment no-fly area.
Since the sizes and shapes of the different fragment no-fly areas obtained in step S502 are irregular, it is difficult to solve each straight-line feasible path between the different fragment no-fly areas, in order to quickly solve each straight-line feasible path between all the fragment no-fly areas from the starting point position to the end point position, an circumscribed circle is made for each fragment no-fly area, the area determined by the circumscribed circle is used as a circular no-fly area, and on the basis of the obtained circular no-fly area, all the straight-line feasible paths between the starting point position and the end point position are solved by using a method of an internal and external common tangent of a circle.
Step S503 corresponds to step S102 in embodiment 1.
The invention adopts a method of circle inside and outside tangent lines to solve all the straight feasible paths from the starting position to the end position, and solves all the straight feasible paths and feasible route points from the starting position to the end position, which is a circular process. Firstly, starting from a starting point position, searching all current straight-line feasible paths starting from the starting point position, which is a first cycle; secondly, respectively taking the end point of each current straight-line feasible path obtained by the first circulation as the current position, starting from the current position, and solving the next straight-line feasible path corresponding to all current straight-line feasible paths obtained by the first circulation as all current straight-line feasible paths obtained by the circulation; and thirdly, respectively taking the end points of all the current straight-line feasible paths as the current positions, searching straight-line feasible paths starting from the circular no-fly area where the current position is located, and circulating until the obtained end points of all the current straight-line feasible paths are the end point positions where the unmanned aerial vehicle needs to finish tasks, wherein at the moment, the fact that the searching of all the straight-line feasible paths from the starting point position to the end point positions is finished is shown.
Next, a flow of a method for finding all straight feasible paths from the starting position to the ending position based on the divided circular no-fly area is shown in fig. 6, and the method may include:
step S601: initializing a starting position, an end position, a current position, a zone bit, a path memory, a straight line feasible path counter and a circular no-fly zone counter.
The variables initialized in this step include: the system comprises a starting position, an end position, a current position, a zone bit, a path memory, a straight line feasible path counter and a circular no-fly zone counter; the starting position is a starting position when the unmanned aerial vehicle executes a task, and the end position is a position when the unmanned aerial vehicle executes the task; the flag bit is used for indicating whether a common tangent line between two circular no-fly areas intersects with other circular no-fly areas or not, if the flag bit is 1, the common tangent line intersects with other circular no-fly areas, and if the flag bit is not 1, the common tangent line does not intersect with other circular no-fly areas, the path memory comprises a straight line feasible path memory, a current straight line feasible path memory and a non-feasible path memory, the straight line feasible path memory is used for storing straight line feasible paths, the current straight line feasible path memory is used for storing all straight line feasible paths obtained in a one-time circulation process, and the non-feasible path memory is used for storing non-feasible paths; the straight line feasible path counter is used for displaying the number of the current straight line feasible paths of the next step of straight line feasible paths which are solved in the current straight line feasible paths stored in the current straight line feasible path memory; the circular no-fly zone counter automatically displays the number of circular no-fly zones for which the straight-line feasible path has been solved until the current position.
At the initial moment, the flag bit is not 1, the number of the current linear path memories is 0, the display value of the linear feasible path counter is 0, and the display value of the circular no-fly area counter is 0.
Step S602: and reading the starting position and the end position, and taking the starting position as the current position.
And acquiring the position of the unmanned aerial vehicle to take off, and taking the starting point position of the unmanned aerial vehicle to take off as the current position. And meanwhile, the position of the unmanned aerial vehicle, which needs to execute the task, is obtained, and the starting position and the end position are used as initial quantities of the air route planning algorithm.
Step S603: and making a current connecting line between the current position and the terminal position.
In this step, a connection line between the current position and the end position is made, the connection line is used as the current connection line, whether the current connection line passes through other circular no-fly areas is reflected through a flag bit, if the flag bit is 1, the current connection line passes through other circular no-fly areas, and if the flag bit is not 1, the current connection line does not pass through other circular no-fly areas.
Step S604: it is determined whether the flag bit is 1, if not, step S605 is executed, and if so, step S607 is executed.
The zone bit is used for displaying whether the current connecting line passes through other circular no-fly areas, if so, the zone bit is 1, and if not, the zone bit is not 1.
It should be noted that, if the current connection line passes through the circular no-fly area, the flag bit may be another value, as long as the numerical value of the flag bit can reflect whether the current connection line passes through the circular no-fly area.
The step is to judge whether the current connecting line passes through the circular no-fly area between the current position and the end position, if the flag bit is 1, the path determined by the current connecting line is infeasible, therefore, the infeasible path is saved, and a straight-line feasible path starting from the circular no-fly area where the current position is located needs to be searched again.
Step S605: and judging whether the current position is the end position, if so, determining that the current connecting line is the current common tangent line, and turning to the step S612, otherwise, executing the step S606.
This step is an action executed when the current connecting line between the current position and the end position determined in step S604 does not pass through the circular no-fly area.
Step S606: and calculating a current common tangent line from the end point position to the circular no-fly area where the current position is located, taking the current common tangent line as a current connecting line, and executing step S604.
In this step, first, a common tangent line is made from the end point position to the circular no-fly area where the current position is located, and since the end point position is a point, there are two common tangent lines at this time; then, a common tangent line passing through the current position side is selected from the two common tangent lines as a current common tangent line, and the current common tangent line is used as a current connection line, and the process proceeds to step S604 and continues to be executed.
Step S607: finding the circular no-fly area closest to the current position in the circular no-fly area through which the current connecting line passes, calculating a common tangent line between the circular no-fly area where the current position is located and the circular no-fly area closest to the current position, determining the common tangent line passing through the current position side in the common tangent lines as the current common tangent line, and entering step S608.
Specifically, first, a circular no-fly area through which a current connecting line passes needs to be found, and a circular no-fly area closest to a current position is determined; then, a common tangent between the circular no-fly area where the current position is located and the circular no-fly area closest to the current position is calculated.
If the current position is not the starting position, four common tangent lines are selected, and two common tangent lines passing through the current position side of the circular no-fly area are selected as the current common tangent lines; and if the current position is the starting position, calculating a common tangent line from the current position to a circular no-fly area closest to the current position, wherein the common tangent line is the current common tangent line.
Step S608: and judging whether the current flag bit is 1, if so, executing the step 609, and if not, executing the step 610.
Whether the current flag bit in this step is 1 indicates whether the current common tangent obtained in step S607 passes through other circular no-fly regions, and if the flag bit is 1, it indicates that the current common tangent passes through other circular no-fly regions, step S609 is executed; if the flag bit is not 1, indicating that the current common tangent does not pass through other circular no-fly regions, step S610 is performed.
Step S609: the current common tangent is stored in the infeasible path memory, the current common tangent is used as the current connection line, and the process goes to step S607.
After the step S607 is shifted, a common tangent line between the circular no-fly area where the current position is located and the circular no-fly area closest to the current position is continuously calculated, a common tangent line passing through the current position side in the common tangent line is determined to be the current common tangent line, then, the step S608 is executed, the process is sequentially circulated until the step S608 determines that the flag bit is not 1, the process jumps out of the step, and the process proceeds to the step S610.
Step S610: and judging whether the straight-line feasible path adjacent to the current common tangent line is intersected with the path determined by the current common tangent line, if so, entering step S611, and if not, entering step S612.
This step is to determine whether there is a fly-around path between the current common tangent line and the straight-line feasible path adjacent to the current common tangent line, and details of the fly-around path are described in detail in embodiment 1 and are not described herein again.
Step S611: and (4) taking the circular no-fly area where the starting point of the straight line feasible path adjacent to the current common tangent line is located and the external common tangent line on the current common tangent line side between the circular no-fly areas where the end points of the paths determined by the current common tangent line are located as the current connecting line, turning to the step (S604) to continue to execute the steps, and circulating the steps until the judgment result of the step (S610) is negative, and entering the step (S612).
Step S612: determining the current common tangent line as the current straight-line feasible path, storing the current straight-line feasible path in the current straight-line feasible path memory, adding the number of the current straight-line feasible path memory to the number of the straight-line feasible paths at the moment, and adding the number of the feasible waypoints to 2 times of the number of the current straight-line feasible paths at the moment, and then entering step S613.
Step S613: and judging whether the number of the linear feasible path counters is added with 1 to be greater than the linear feasible path numerical value, if not, entering the step S614, and if so, entering the step S615.
In this step, the straight-line feasible path value refers to the number of all the current straight-line feasible paths obtained in the last cycle. The straight-line feasible path counter represents that in the process of the current cycle, the number of the current straight-line feasible paths from all the current straight-line feasible paths obtained in the previous cycle to the next straight-line feasible path is solved. If the number of the linear feasible path counters plus 1 is greater than the number of the linear feasible paths, all the current linear feasible paths obtained in the circulation of the previous step are solved to the linear feasible paths of the next step; if not, the current straight-line feasible path which is not solved for the next straight-line feasible path still exists in all the current straight-line feasible paths obtained in the last cycle.
Step S614: and acquiring a current straight-line feasible path of the next straight-line feasible path which is not solved in the feasible path memory, taking the end point of the acquired straight-line feasible path as the current position, adding 1 to the straight-line feasible path counter, and executing the step S603.
Step S615: and resetting the straight line feasible path counter, wherein the number value of the straight line feasible path is the number of the current straight line feasible path memory, the straight line feasible paths in the current straight line feasible path memory are stored in the feasible path memory, and the current straight line feasible path memory is reset.
In this step, the straight-line feasible path memory is used for storing the next straight-line feasible path corresponding to the current straight-line feasible path obtained in each cycle process.
Step S616: it is determined whether the number of the circular no-fly regions counter is smaller than the number of the circular no-fly regions, and if so, step S613 is executed, and if not, step S617 is executed.
Step S617: and ending the flow.
This step indicates that the determination of the straight feasible paths and feasible waypoints between all circular no-fly areas from the starting position to the ending position is completed.
All the straight-line feasible paths from the starting point position to the end point position are found through the above steps S601 to S617, and the straight-line feasible paths are stored in the straight-line feasible path memory. The method for finding all feasible routes at the end position from the starting position according to the straight feasible paths stored in the straight feasible path memory comprises the following steps: firstly, starting from a starting point position, searching all straight-line feasible paths starting from the starting point position stored in a straight-line feasible memory; then, starting to search the end points of all the straight-line feasible paths at the moment, taking the end points as the straight-line feasible paths of the starting point, and according to the steps, until the end points of all the straight-line feasible paths are found as the end point positions; and finally, from the starting position to the end position along the found straight feasible path, wherein each route from the starting position to the end position is a feasible route.
The process of planning the optimal route between the starting position and the ending position according to all feasible routes and the route point is the same as step S105 in the embodiment, and is not described herein again.
The embodiment of the invention describes the specific implementation steps of the route planning method in detail, the steps can be placed in an unmanned aerial vehicle, when the unmanned aerial vehicle encounters an unplanned no-fly area in the flying process, a sensor on the unmanned aerial vehicle acquires the current position of the unmanned aerial vehicle, and according to the method of the embodiment of the invention, the optimal route from the current position to the end position is found within limited time so as to complete a specific task within the specified time.
Example 3
The embodiment of the invention discloses a route planning device based on no-fly zone restriction, which is integrated on an unmanned aerial vehicle and used for the unmanned aerial vehicle to plan a route from a starting position to an end position, please refer to fig. 7, and the device can comprise:
the acquiring unit 701 is configured to segment a crossing area and a dense small area in a no-fly area between a starting position and an ending position of the unmanned aerial vehicle to obtain different segmented no-fly areas.
A first determining unit 702, configured to determine, as a circular no-fly area, an area included in an circumscribed circle of each slice no-fly area.
A calculating unit 703, configured to solve each straight feasible path and feasible waypoint between the starting point position and the end point position by using a method of an inner common tangent and an outer common tangent of a circle between the circular no-fly areas, where the feasible waypoint is an intersection of the straight feasible path and the circular no-fly area.
A second determining unit 704, configured to determine all feasible routes from the starting location to the ending location according to the straight feasible route.
A third determining unit 705, configured to plan an optimal route from the starting position to the ending position according to all feasible routes and the feasible route points.
Specifically, in this embodiment of the present invention, the third determining unit may include: the device comprises a first determining subunit and a second determining subunit. Wherein,
and the first determining subunit is used for selecting the air route with the shortest path from all the feasible air routes as the optimal air route.
And the second determining subunit is configured to determine, according to whether the attribute of the feasible route point is an entry tangent point or an exit tangent point, a flight mode between two adjacent feasible route points in two adjacent straight-line feasible paths in the optimal route, so as to obtain an optimal route from the starting point position to the end point position.
Specifically, in the embodiment of the present invention, the second determining subunit is specifically configured to:
determining to adopt a linear flight mode under the condition that two adjacent feasible route points in two adjacent linear feasible paths in the optimal route are from an exit tangent point to an entry tangent point; or, if two adjacent feasible route points in two adjacent straight feasible paths in the optimal route are from an entry tangent point to an exit tangent point, an arc flight mode is adopted, the exit tangent point is a tangent point of a straight feasible path leaving the circular no-fly area according to the direction of the feasible route and the circular no-fly area, and the entry tangent point is a tangent point of a straight feasible path flying into the circular no-fly area according to the direction of the feasible route and the circular no-fly area.
Specifically, in this embodiment of the present invention, the calculating unit may include: the system comprises an acquisition subunit, an execution subunit and a trigger subunit. Wherein,
and the acquisition subunit is used for acquiring the starting point position as the current position.
An execution subunit, configured to determine, for the current position, a current connection line between the current position and the end point position;
judging whether the current connecting line passes through the circular no-fly area;
if the current connecting line passes through the circular no-fly area, determining a circular no-fly area which is closest to the current position on the current connecting line, and determining a current common tangent line which passes through the current position side between the circular no-fly area where the current position is located and the circular no-fly area which is closest to the current position;
judging whether the current common tangent line passes through the circular no-fly area;
if not, judging whether a straight line feasible path adjacent to the current common tangent line is intersected with the path determined by the current common tangent line; if not, determining that the current common tangent is the current straight-line feasible path.
And the triggering subunit is used for searching the straight-line feasible path starting from the current position by taking the end point of the current straight-line feasible path as the current position until all the straight-line feasible paths between the starting point position and the end point position are determined.
In an embodiment of the apparatus of the present invention, the execution subunit may further be configured to:
if the current connecting line does not pass through the circular no-fly area, judging whether the current position is the end point position, and if so, determining that the current connecting line is the current straight line feasible path;
if the current position is not the end position, calculating a current common tangent from the end position to the current position side of the circular no-fly area where the current position is located;
judging whether the current common tangent line passes through the circular no-fly area;
if the current common tangent line passes through the circular no-fly area, taking the current common tangent line as a current connecting line, and executing the step that the current connecting line passes through the circular no-fly area;
and if not, determining that the current connecting line is the current straight-line feasible path.
Judging whether a straight-line feasible path adjacent to the current common tangent line intersects with a path determined by the current common tangent line, and may further include:
and if the current connection line is intersected with the circular no-fly area, taking an external common tangent line on the side of the current common tangent line between the circular no-fly area where the starting point of the straight line feasible path adjacent to the current common tangent line is located and the circular no-fly area where the end point of the path determined by the current common tangent line is located as the current connection line, and executing the step of judging whether the current connection line passes through the circular no-fly area.
In the device embodiment of the invention, an unmanned aerial vehicle firstly obtains a fragmentation no-fly area from a starting position to an end position through an obtaining unit; then, determining an circumscribed circle of each sliced no-fly area as a circular no-fly area through a first determining unit, so as to solve all straight-line feasible paths and feasible route points from the starting position to the end position by adopting a method of inner and outer common tangents of the circles through a calculating unit in the following process; and finally, determining all feasible routes from the starting position to the end position through a second determining unit and a third determining unit, determining one route with the shortest route among all feasible routes as an optimal route, and determining the optimal route from the starting position to the end position of the unmanned aerial vehicle according to the optimal route and the feasible route point attribute in the optimal route.
The previous description of the disclosed embodiments is provided to enable any person skilled in the art to make or use the present invention. Various modifications to these embodiments will be readily apparent to those skilled in the art, and the generic principles defined herein may be applied to other embodiments without departing from the spirit or scope of the invention. Thus, the present invention is not intended to be limited to the embodiments shown herein but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.
Finally, it should also be noted that, herein, relational terms such as first and second, and the like may be used solely to distinguish one entity or action from another entity or action without necessarily requiring or implying any actual such relationship or order between such entities or actions. Also, the terms "comprises," "comprising," or any other variation thereof, are intended to cover a non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements does not include only those elements but may include other elements not expressly listed or inherent to such process, method, article, or apparatus. Without further limitation, an element defined by the phrase "comprising an … …" does not exclude the presence of other identical elements in a process, method, article, or apparatus that comprises the element.