A rotor unmanned aerial vehicle path planning method for non-standard convex quadrilateral region

Abstract

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

Description

Technical Field

[0001] The present 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] Trajectory planning algorithms can be categorized into two main categories: traditional classical algorithms and modern intelligent algorithms. The former primarily include dynamic programming, derivative correlation methods, and optimal control methods; the latter primarily include heuristic optimization search, genetic algorithms, neural networks, and swarm intelligence algorithms. A typical trajectory planning process involves three steps: first, considering various threats and constraints to construct a trajectory planning model; second, using optimization algorithms to find the optimal trajectory; and finally, performing 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 trajectory planning method for a rotary-wing UAV in a non-standard convex quadrilateral area in response to 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 angle;

[0008] 2) Obtain the flight altitude of the UAV and calculate the UAV unit search area based on the camera's field of view;

[0009] 3) Mapping the mission area of ​​the UAV 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 mission according to the minimum enclosing 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 point;

[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, the point closest to the southwest side is used as the GPS reference point and the GPS coordinate is marked 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; where 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 solution, 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 UAV 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 enclosing rectangle of the mission area, and the track point trajectory is calculated point by point according to the minimum enclosing rectangle so that the total search area of ​​all track points completely covers the area enclosed by the minimum enclosing 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 the point P t Whether it is located in the mission area;

[0027] 6.2) Based on the set threshold, retain some track points outside the mission area boundary;

[0028] Calculate the distance between all track points outside the mission area and the mission area boundary, 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 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 , substitute 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] The method of the present invention automatically plans the search track points when the given mission area is an arbitrary convex quadrilateral. At the same time, it 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 diagram of a method according to an embodiment of the present invention;

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

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

[0036] Figure 4 2. It is a schematic diagram of the area method for excluding track points outside the area 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 according to an embodiment of the present invention;

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

[0039] In order to make the purpose, technical solutions 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 intended to limit the present invention.

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

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

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

[0043] FOV is the angle between the two ends of the camera's viewing cone. The focal length f is the distance from the lens to the "sensor." 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 aspect ratio is 16:9. The calculated 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 using the same method.

[0044] 2) Obtain the flight altitude of the UAV and calculate the search area of ​​the UAV unit;

[0045] The flight altitude of the UAV is determined based on the pre-specified flight altitude or the actual altitude obtained after takeoff. The actual area in the image captured by the camera during flight is calculated based on the camera FOV and the image frame size, which is the UAV 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 drones have limited flight range, and their mission radius is typically less than 6 km. The vertices of the area are typically given as GPS coordinates. Before calculating and selecting track points, the mission area must be mapped to a rectangular coordinate system. Once the track points are calculated, the GPS coordinates of the mission track points can be inferred from their relative positions in the 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, the point closest to the southwest is used as the GPS reference point and the GPS coordinate is marked as lon base ,lat base . 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 points P1, P2, P3, and P4.

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

[0051] like Figure 2 As shown in the figure, 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] Based on the UAV unit search area and initial flight parameters, the portion 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 drone's takeoff position and flight direction, with the four sides of the mission area's 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 to filter out all track points in the mission area.

[0057] Because 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 mission area boundary, 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 , substitute 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 results of the present invention's method with existing methods. Each row in the figure represents a set of trajectory planning results. The red area in the first column represents a given irregular convex quadrilateral area, the second column shows the results after trajectory planning using the minimum circumscribed rectangle method, and the third column shows the results after trajectory planning using this method. Compared to the flight paths in the second column, using this method to eliminate redundant points reduced the total flight distance of the drone by 27.4%, 37.1%, 31.0%, and 36.6%, respectively. This effectively reduced ineffective search flights outside the mission area and improved the drone's regional search efficiency.

[0062] Task execution;

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

[0064] 1. Obtain the longitude, latitude, altitude, target heading, and nose direction of the takeoff point, set various flight data for 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 (hovering, rotating the gimbal, performing target detection and identification, 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 based on 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 such 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 angle; 2) Obtain the flight altitude of the UAV and calculate the UAV unit search area based on the camera's field of view; 3) Mapping the mission area of ​​the UAV 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 mission according to the minimum enclosing 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 point; 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 in a non-standard convex quadrilateral area according to claim 1, characterized in that: 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.

3. The method for trajectory planning of a rotary-wing UAV in a non-standard convex quadrilateral area according to claim 1, characterized in that: 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 captured by the camera when the UAV is flying is calculated as the UAV unit search area.

4. The method for trajectory planning of a rotary-wing UAV in a non-standard convex quadrilateral area according to claim 1, characterized in that: In step 3), the mission area where the UAV 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, the point closest to the southwest side is used as the GPS reference point and the GPS coordinate is marked 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; where 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 in a non-standard convex quadrilateral area according to claim 1, characterized in that: In 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 in a non-standard convex quadrilateral area according to claim 1, characterized in that: In step 5), the track points of the UAV 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 enclosing rectangle of the mission area, and the track point trajectory is calculated point by point according to the minimum enclosing rectangle so that the total search area of ​​all track points completely covers the area enclosed by the minimum enclosing rectangle.

7. The method for trajectory planning of a rotary-wing UAV in a non-standard convex quadrilateral area 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 the point P t Whether it is located in the mission area; 6.2) Based on the set threshold, retain some track points outside the mission area boundary; Calculate the distance between all track points outside the mission area and the mission area boundary, 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 in a non-standard convex quadrilateral area 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 , substitute 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