Rotor unmanned aerial vehicle flight path planning method for non-standard convex quadrilateral area

Abstract

The invention discloses a rotor unmanned aerial vehicle flight path planning method for a non-standard convex quadrilateral area, and the method comprises the following steps: 1), obtaining the parameters of an unmanned aerial vehicle camera, and calculating the field angle of the camera; 2) acquiring the flight height of the unmanned aerial vehicle for executing the task, and calculating an unmanned aerial vehicle unit search area according to a camera field angle; 3) mapping a task area of task execution of the unmanned aerial vehicle to a rectangular plane coordinate system; 4) acquiring a minimum enclosing rectangle of the task area on the rectangular plane coordinate system; 5) according to the minimum enclosing rectangle, calculating the flight path point of the unmanned aerial vehicle for executing the task; 6) screening track points according to the task area; 7) acquiring the track points of the track points in the task area under the rectangular plane coordinate system, and calculating the GPS coordinates of all the track points based on the reference GPS points; and 8) obtaining GPS coordinates of all track points meeting the conditions, and obtaining a task track. According to the method, the unmanned aerial vehicle can more efficiently complete searching of the non-standard convex quadrilateral area.

Description

Technical Field

[0001] The invention relates to a path planning technology, and in particular to a rotor UAV trajectory planning method for a non-standard convex quadrilateral area. Background Art

[0002] Trajectory planning refers to finding the optimal trajectory of a moving object from its initial point to its target point based on certain performance indicators under given constraints.

[0003] The trajectory planning algorithms can be divided into two categories: traditional classical algorithms and modern intelligent algorithms. Among them, the former mainly includes dynamic programming, derivative correlation method, and optimal control method; the latter mainly includes heuristic optimization search, genetic algorithm, neural network, swarm intelligence algorithm, etc. Typical trajectory planning includes three steps: first, consider various threats and constraints and build a trajectory planning model; second, use optimization algorithm to find the optimal trajectory; finally, perform trajectory smoothing.

[0004] When the given mission area is a non-standard convex quadrilateral, if each track point is calculated according to the minimum circumscribed rectangle method, many track points outside the area will appear, affecting the UAV search efficiency. Summary of the invention

[0005] The technical problem to be solved by the present invention is to provide a rotor UAV trajectory planning method for a non-standard convex quadrilateral area in view of the defects in the prior art.

[0006] The technical solution adopted by the present invention to solve the technical problem is: a method for trajectory planning of a rotary-wing UAV for a non-standard convex quadrilateral area, comprising the following steps:

[0007] 1) Obtain the drone camera parameters and calculate the camera field of view;

[0008] 2) Obtain the flight altitude of the drone to perform the mission and calculate the drone unit search area based on the camera field of view;

[0009] 3) Mapping the mission area where the UAV performs the mission to a plane rectangular coordinate system; the mission area is a non-standard convex quadrilateral;

[0010] 4) Obtain the minimum circumscribed rectangle of the task area in a rectangular coordinate system;

[0011] 5) Calculate the track points of the UAV’s mission according to the minimum circumscribed rectangle;

[0012] 6) Filter track points according to the mission area;

[0013] 7) For the track points in the mission area, obtain their track points in the plane rectangular coordinate system, and calculate the GPS coordinates of all track points based on the reference GPS points;

[0014] 8) Get the GPS coordinates of all track points that meet the conditions and obtain the mission track.

[0015] According to the above scheme, in step 1), the actual focal length and zoom factor information of the drone are obtained according to the camera parameters used to perform the mission, and then the field of view of the camera, i.e., FOV, is calculated and obtained according to the field of view angle calculation formula.

[0016] According to the above scheme, in step 2), the pre-specified flight altitude or the actual height above the ground obtained after takeoff is used as the flight altitude of the UAV to perform the mission. According to the camera FOV and the image frame size, the size of the actual area in the image taken by the camera looking down when the UAV is flying is calculated as the UAV unit search area.

[0017] According to the above scheme, in step 3), the mission area where the UAV performs the mission is mapped to a plane rectangular coordinate system, as follows:

[0018] Given the GPS coordinates of the four vertices GP1, GP2, GP3, and GP4 of the convex quadrilateral task area, take the point closest to the southwest as the GPS reference point and mark the GPS coordinates as lon base ,lat base ;

[0019] according to Calculate the east-west distance difference between the four vertices and the GPS reference point, press Calculate the north-south distance difference between the four vertices and the GPS reference point; R earth is the radius of the Earth;

[0020] Map all vertices to the first quadrant of the rectangular coordinate system to obtain four points: P1, P2, P3, and P4.

[0021] According to the above scheme, in step 4), the minimum circumscribed rectangle of the non-standard convex quadrilateral is obtained according to the rotating caliper algorithm.

[0022] According to the above scheme, in step 5), the track points of the drone performing the mission are calculated as follows:

[0023] According to the UAV unit search area and the initial flight parameters, the track planning is carried out for the part enclosed by the minimum circumscribed rectangle of the mission area, and the track points are calculated point by point according to the minimum circumscribed rectangle so that the total search area of ​​all track points completely covers the area enclosed by the minimum circumscribed rectangle.

[0024] According to the above scheme, in step 6), selecting the track points according to the mission area includes the following steps:

[0025] 6.1) Use the area method to exclude track points outside the mission area;

[0026] Take any point P in the plane t Compare the area of ​​the triangle formed by the four vertices of the quadrilateral area of ​​the task area with the area of ​​the quadrilateral area of ​​the original task area to determine point P t Whether it is located in the mission area;

[0027] 6.2) According to the set threshold, some track points outside the mission area boundary are retained;

[0028] Calculate the distance between all track points outside the mission area and the boundary of the mission area, and use the set threshold to filter out track points outside the boundary as extended track points and add them to the UAV mission track.

[0029] According to the above scheme, in step 7), after all plane rectangular coordinate system track points that meet the conditions are obtained, the east-west distance difference D of all track points relative to the reference GPS point is calculated according to the distance formula ew The north-south distance difference D ns , substituting the distance difference into the formula The GPS coordinates of all track points can be calculated.

[0030] The beneficial effects produced by the present invention are:

[0031] When the given mission area is an arbitrary convex quadrilateral, the method of the present invention automatically plans the search track points, and can effectively exclude redundant track points outside the search mission area to obtain the optimal search trajectory, so that the UAV can more efficiently complete the search of non-standard convex quadrilateral areas. BRIEF DESCRIPTION OF THE DRAWINGS

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

[0033] Figure 1 is a schematic structural diagram of an embodiment of the present invention;

[0034] Figure 2 is a schematic diagram of a minimum circumscribed rectangle according to an embodiment of the present invention;

[0035] Figure 3 This is a schematic diagram of the minimum circumscribed rectangle trajectory planning of an embodiment of the present invention;

[0036] Figure 4 is a schematic diagram of an area method for excluding track points outside a region according to an embodiment of the present invention;

[0037] Figure 5 This is a schematic diagram of track planning after excluding track points outside the mission area in an embodiment of the present invention;

[0038] Figure 6 2 is a schematic diagram for comparing the trajectory planning effects of an embodiment of the present invention. DETAILED DESCRIPTION

[0039] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not used to limit the present invention.

[0040] like Figure 1 As shown, a method for trajectory planning of a rotary-wing UAV for a non-standard convex quadrilateral region comprises the following steps:

[0041] 1) Obtain the drone camera parameters and calculate the camera field of view;

[0042] According to the camera parameters used by the drone to perform the mission, the actual focal length and zoom factor information are obtained, and then the field of view of the camera, i.e. FOV, is calculated and obtained according to the field of view angle calculation formula;

[0043] FOV is the angle between the two ends of the camera's visual cone, the focal length f is the distance from the lens to the "sensor", and the imaging frame is a rectangle with a length of 2w and a width of 2h. For the DJI M300 rotary wing drone equipped with the Zenmuse H20T camera, the camera focal length f is 23.7mm, and the camera output image frame ratio is 16:9. It is calculated that the film frame of the camera is 36mm×20.25mm. According to the formula The horizontal and vertical field of view angles can be calculated separately. If the drone is equipped with other types of cameras for area search, the calculation can be performed in the same way.

[0044] 2) Obtain the flight altitude of the drone to perform the mission and calculate the drone unit search area;

[0045] The flight altitude of the drone is determined based on the pre-specified flight altitude or the actual altitude obtained after takeoff. The actual area in the image taken by the camera when the drone is flying is calculated based on the camera FOV and the image frame size, i.e., the drone unit search area.

[0046] After obtaining the drone's HFOV and VFOV, the actual area in the camera's field of view can be calculated based on the mission's given altitude or the drone's real-time altitude. If the drone's flight altitude is F_H, the length of the unit search area is H20T_W, and the width is H20T_H, substitute the formula The actual area size of the image obtained by the drone can be calculated when the camera is in the downward search state.

[0047] Rotary-wing UAVs have limited endurance, and their mission radius is generally less than 6km. The vertices of the area are generally given in the form of GPS coordinates. Before calculating and selecting the track points, the mission area must be mapped to a plane rectangular coordinate system. After the track points are calculated, the GPS coordinates of the mission track points can be inferred based on the relative positions of the track points in the plane rectangular coordinate system.

[0048] 3) Mapping the task area to a plane rectangular coordinate system; the task area is a non-standard convex quadrilateral;

[0049] Given the GPS (radian) coordinates of the four vertices GP1, GP2, GP3, and GP4 of the convex quadrilateral in the mission area, take the point closest to the southwest as the GPS reference point and mark the GPS coordinates as lon base ,lat base . You can press Calculate the east-west distance difference between other points and the GPS reference point, press Calculate the north-south distance difference, where R earth is the radius of the earth. Then all vertices are mapped to the first quadrant of the rectangular coordinate system to obtain four points: P1, P2, P3, and P4.

[0050] 4) Calculate the minimum circumscribed rectangle of the task area in the plane rectangular coordinate system;

[0051] like Figure 2 As shown, according to the positions of the four points P1, P2, P3, and P4, an irregular convex quadrilateral is formed in the plane rectangular coordinate system. According to the rotating caliper algorithm, the minimum circumscribed rectangle of the irregular convex quadrilateral can be obtained. The four vertices of the rectangle are RP1, RP2, RP3, and RP4 respectively.

[0052] 5) Calculate track points;

[0053] According to the UAV unit search area and initial flight parameters, the part enclosed by the minimum circumscribed rectangle of the mission area is calculated point by point, so that the total search area of ​​all track points completely covers the area enclosed by the minimum circumscribed rectangle;

[0054] The drone unit search area is a regular rectangle with a length of H20T_W and a width of H20T_H. Based on the take-off position and flight direction of the drone, with the four sides of the mission area circumscribed rectangle as the boundaries, all track points traversing the area enclosed by the circumscribed rectangle can be calculated. The schematic diagram is shown in Figure 3 .

[0055] 6) Exclude track points outside the mission area;

[0056] like Figure 4 As shown, for the non-standard convex quadrilateral area P1P2P3P4, the judgment point P tWhether it is within the region can be determined by the area method. So the total area of ​​the quadrilateral is Calculate S t =S △ P1P2P t +S △ P2P3P t +S △ P3P4P t +S △ P4P5P t , then compare S t The relationship between the size of point P and S can be used to determine the point P. t Whether it is within the area enclosed by the convex quadrilateral. All track points in the minimum circumscribed rectangle of the mission area are calculated one by one, and all track points in the mission area can be screened out.

[0057] In the actual mission process, when the search target moves near the boundary of the area, it may slightly exceed the boundary range, so the screening conditions for track points near the boundary should be appropriately relaxed. In the plane rectangular coordinate system, the four sides of the mission area can be represented by Ax+By+C=0, and the coordinates of the track points can be represented by x i ,y i According to the formula After calculating the distance between all track points outside the mission area and the boundary of the mission area, the track points outside the boundary are selected as extended track points based on a certain threshold and added to the UAV mission track;

[0058] Finally, the track of the mission area is obtained as follows Figure 5 ;

[0059] 7) After obtaining all the plane rectangular coordinate system track points that meet the conditions, the east-west distance difference D of all track points relative to the reference GPS point can be obtained according to the distance formula ew The north-south distance difference D ns , substituting the distance difference into the formula The GPS coordinates of all track points can be calculated;

[0060] 8) Get the GPS coordinates of all track points and obtain the mission track.

[0061] Figure 6This is a schematic diagram comparing the trajectory planning effects of the method of the present invention and the existing methods. Each line in the figure is a set of trajectory planning effects, where the red area in the first column is a given irregular convex quadrilateral area, the second column is the effect diagram after trajectory planning according to the minimum circumscribed rectangle method, and the third column is the effect diagram after trajectory planning according to this method. Compared with the flight routes in the second column, after using this method to eliminate redundant points, the total flight distance of the drone is reduced by 27.4%, 37.1%, 31.0% and 36.6% respectively, effectively reducing invalid search flights outside the mission area and improving the efficiency of drone regional search.

[0062] Task execution;

[0063] The waypoint mission is the most commonly used and most flexible flight mode in rotary wing drones. You can plan multiple mission points for the drone's flight offline as needed, and then upload these mission points to the drone. By calling the DJI OSDK interface, the drone can be switched to the waypoint mission mode. After that, the drone will fly according to the current mission. The specific execution process is as follows:

[0064] 1. Obtain the longitude, latitude, altitude, target heading, nose direction and other information of the take-off point, set various flight data of the drone, and the drone takes off and flies to the first track point.

[0065] 2. Determine whether the distance between the current position of the drone and the target track point is less than the threshold (2 meters), and then determine whether the drone has reached the track point.

[0066] 3. If the drone reaches the current track point, it will execute the mission command of the current track point (hover, rotate the gimbal, perform target detection and recognition, etc.). After completing the corresponding mission, it will obtain the coordinates of the next track point.

[0067] 4. Set the next track point as the current flight target point of the drone. Set the current flight target position of the drone according to the longitude, latitude, altitude and heading of this track point, and control the drone to fly to the next track point.

[0068] 5. Repeat steps 2-4 until all the tasks at the track points have been completed, and then return home.

[0069] It should be understood that those skilled in the art can make improvements or changes based on the above description, and all these improvements and changes should fall within the scope of protection of the appended claims of the present invention.

Claims

1. A method for trajectory planning of a rotary-wing UAV in a non-standard convex quadrilateral area, characterized in that: The following steps are involved: 1) Obtain the drone camera parameters and calculate the camera field of view; 2) Obtain the flight altitude of the drone to perform the mission and calculate the drone unit search area based on the camera field of view; 3) Mapping the mission area where the UAV performs the mission to a plane rectangular coordinate system; the mission area is a non-standard convex quadrilateral; 4) Obtain the minimum circumscribed rectangle of the task area in a rectangular coordinate system; 5) Calculate the track points of the UAV’s mission according to the minimum circumscribed rectangle; 6) Filter track points according to the mission area; 7) For the track points in the mission area, obtain their track points in the plane rectangular coordinate system, and calculate the GPS coordinates of all track points based on the reference GPS points; 8) Get the GPS coordinates of all track points that meet the conditions and obtain the mission track.

2. The method for trajectory planning of a rotary-wing UAV for a non-standard convex quadrilateral region according to claim 1, characterized in that: In the step 1), the actual focal length and zoom factor information of the drone are obtained according to the camera parameters used for the mission, and then the field of view of the camera, i.e., FOV, is calculated and obtained according to the field of view angle calculation formula.

3. The method for trajectory planning of a rotary-wing UAV for a non-standard convex quadrilateral region according to claim 1, characterized in that: In the step 2), the pre-specified flight altitude or the actual height above the ground obtained after takeoff is used as the flight altitude of the UAV to perform the mission. According to the camera FOV and the image frame size, the size of the actual area in the image taken by the camera looking down when the UAV is flying is calculated as the UAV unit search area.

4. The method for trajectory planning of a rotary-wing UAV for a non-standard convex quadrilateral region according to claim 1, characterized in that: In step 3), the mission area where the drone performs the mission is mapped to a plane rectangular coordinate system, as follows: Given the GPS coordinates of the four vertices GP1, GP2, GP3, and GP4 of the convex quadrilateral task area, take the point closest to the southwest as the GPS reference point and mark the GPS coordinates as lon base ,lat base ; according to Calculate the east-west distance difference between the four vertices and the GPS reference point, press Calculate the north-south distance difference between the four vertices and the GPS reference point; R earth is the radius of the Earth; Map all vertices to the first quadrant of the rectangular coordinate system to obtain four points: P1, P2, P3, and P4.

5. The method for trajectory planning of a rotary-wing UAV for a non-standard convex quadrilateral region according to claim 1, characterized in that: In the step 4), the minimum circumscribed rectangle of the non-standard convex quadrilateral is obtained according to the rotating caliper algorithm.

6. The method for trajectory planning of a rotary-wing UAV for a non-standard convex quadrilateral region according to claim 1, characterized in that: In step 5), the track points of the drone performing the mission are calculated as follows: According to the UAV unit search area and the initial flight parameters, the track planning is carried out for the part enclosed by the minimum circumscribed rectangle of the mission area, and the track points are calculated point by point according to the minimum circumscribed rectangle so that the total search area of ​​all track points completely covers the area enclosed by the minimum circumscribed rectangle.

7. The method for trajectory planning of a rotary-wing UAV for a non-standard convex quadrilateral region according to claim 1, characterized in that: In step 6), selecting the track points according to the mission area includes the following steps: 6.1) Use the area method to exclude track points outside the mission area; Take any point P in the plane t Compare the area of ​​the triangle formed by the four vertices of the quadrilateral area of ​​the task area with the area of ​​the quadrilateral area of ​​the original task area to determine point P t Whether it is located in the mission area; 6.2) According to the set threshold, some track points outside the mission area boundary are retained; Calculate the distance between all track points outside the mission area and the boundary of the mission area, and use the set threshold to filter out track points outside the boundary as extended track points and add them to the UAV mission track.

8. The method for trajectory planning of a rotary-wing UAV for a non-standard convex quadrilateral region according to claim 1, characterized in that: In step 7), after obtaining all plane rectangular coordinate system track points that meet the conditions, the east-west distance difference D of all track points relative to the reference GPS point is calculated according to the distance formula. ew The north-south distance difference D ns , substituting the distance difference into the formula The GPS coordinates of all track points can be calculated.

9. A computer device comprising a memory and a processor, wherein the memory stores a computer program, wherein: When the processor executes the computer program, the steps of the method according to any one of claims 1 to 8 are implemented.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the method according to any one of claims 1 to 8 is implemented.

Images