Automatic emergency route planning method for unmanned aerial vehicle

Abstract

A method for automatic emergency route planning of an unmanned aerial vehicle comprises the following steps: S100, sampling between an unmanned aerial vehicle hangar and a target is carried out according to the terrain, a large number of sampling points are obtained, and the position of the unmanned aerial vehicle hangar and the position of the target serve as the starting point and the ending point of the sampling points; s200, obtaining a large number of pre-waypoints according to the sampling points and topographic data; and S300, optimizing the pre-waypoints to obtain waypoints, and connecting the waypoints in sequence to obtain a route track. According to the method, the response speed and the execution efficiency of the unmanned aerial vehicle in emergency response can be remarkably improved, and powerful technical support is provided for emergency response.

Description

Technical Field

[0001] The invention belongs to the technical field of unmanned aerial vehicles, and in particular relates to a method for automatic emergency route planning of unmanned aerial vehicles. Background Art

[0002] Unmanned Aerial Vehicle (UAV) technology has made significant progress in recent years and is widely used in many fields, including agriculture, logistics, environmental monitoring and emergency response. In emergency response, UAVs have become an important tool for performing monitoring and reconnaissance tasks due to their high mobility, rapid deployment and low cost.

[0003] The effective use of drones in emergency situations faces multiple technical challenges, especially automatic route planning and autonomous flight technology. Emergency response scenarios usually include natural disasters (such as earthquakes, floods, fires), accident rescue (such as traffic accidents, industrial accidents) and public safety events (such as large-scale gatherings). In these scenarios, drones can quickly reach the disaster site and provide real-time image and data support. However, most of the current emergency response drone systems rely on manual operation, and there are problems such as long response time, low efficiency, and difficulty in operation. How to realize the automatic route planning of drones in emergency situations and let the drones fly along the route has become a technical challenge that needs to be solved urgently. Therefore, a technical solution that can realize automatic route planning of drones in unmanned conditions is needed to improve the efficiency and effectiveness of emergency response. Summary of the invention

[0004] In order to achieve timely response and ensure flight safety and emergency speed, and solve the above technical problems, the present invention discloses a method for automatic emergency route planning of a UAV, the method comprising the following steps:

[0005] S100: sampling between the drone hangar and the target according to the terrain to obtain a large number of sampling points, wherein the drone hangar position and the target position are used as the starting point and the ending point of the sampling points;

[0006] S200: obtaining a large number of pre-navigation points according to the sampling points and terrain data;

[0007] S300: Optimizing the pre-waypoints to obtain waypoints, and connecting the waypoints in sequence to obtain a route track.

[0008] Through the above technical solutions, this method can enable the UAV to have a high degree of automation and efficient route planning capabilities during the emergency response process. The system can automatically wake up the UAV without supervision and automatically fly to the target location according to the warning information. The terrain data is used to generate a terrain-like route, and then optimized on this basis, so that the UAV can fly to the target location at the fastest speed. Ensure flight safety and reliability in complex environments. This method can significantly improve the reaction speed and execution efficiency of UAVs in emergency response, and provide strong technical support for emergency response. BRIEF DESCRIPTION OF THE DRAWINGS

[0009] Figure 1 It is a flow chart of a method for automatic emergency route planning of a UAV provided in one embodiment of the present invention;

[0010] Figure 2 is a schematic diagram of sampling point acquisition provided in one embodiment of the present invention;

[0011] Figure 3 is a schematic diagram of sampling points after obtaining height provided in one embodiment of the present invention;

[0012] Figure 4 is a schematic diagram of height extreme value points provided in one embodiment of the present invention;

[0013] Figure 5 is a schematic diagram of a slope maximum provided in one embodiment of the present invention;

[0014] Figure 6 is a schematic diagram of pre-travel points after screening provided in one embodiment of the present invention;

[0015] Figure 7 is a schematic diagram of emergency route waypoints provided in one embodiment of the present invention;

[0016] Figure 8 It is a schematic diagram of connecting lines to generate an emergency route provided in one embodiment of the present invention. DETAILED DESCRIPTION

[0017] In order to make those skilled in the art understand the technical solution disclosed in the present invention, the following will be combined with the embodiments and related appended Figures 1 to 8 , the technical solutions of various embodiments are described, and the described embodiments are part of the embodiments of the present invention, but not all of the embodiments.

[0018] Reference to "embodiments" herein means that a particular feature, structure, or characteristic described in conjunction with the embodiments may be included in at least one embodiment of the present invention. The appearance of the phrase in various locations in the specification does not necessarily refer to the same embodiment, nor is it an independent or alternative embodiment that is mutually exclusive with other embodiments. It will be understood by those skilled in the art that the embodiments described herein may be combined with other embodiments.

[0019] See also Figure 1 In one embodiment, the present invention discloses a method for automatic emergency route planning of a drone, the method comprising the following steps:

[0020] S100: sampling between the drone hangar and the target according to the terrain to obtain a large number of sampling points, wherein the drone hangar position and the target position are used as the starting point and the ending point of the sampling points;

[0021] S200: obtaining a large number of pre-navigation points according to the sampling points and terrain data;

[0022] S300: Optimizing the pre-waypoints to obtain waypoints, and connecting the waypoints in sequence to obtain a route track.

[0023] As far as this embodiment is concerned, in the emergency response scenario, this method can realize the automatic route planning of the drone according to the emergency target, so that the drone can fly to the destination to complete the mission, and ensure the safety and rapid response of the entire mission process. This method can realize automatic route planning in conjunction with the hangar, which can improve the efficiency and effectiveness of the drone emergency response in emergency scenarios. Since the route is optimized according to the terrain, it is guaranteed that the drone can reach the emergency target by the shortest path, ensuring the flight safety of the drone.

[0024] Unmanned operation is achieved by using drones and hangars as equipment. Then when other equipment such as cameras or people give warnings, the drone in the hangar automatically flies to the target location for on-site detection. Before the drone automatically flies, the system needs to automatically complete route planning through the two points of the drone position and the target position. The route planning method is to first sample the area between the hangar and the target according to the terrain to obtain a large number of ground points; then add a relative height to these points and screen the feature points to obtain a large number of pre-flight points; finally, optimize the pre-flight points to a flight route that allows the drone to fly to the destination faster.

[0025] In another embodiment, the sampling points are obtained according to the following steps: the target position and the position of the drone hangar are regarded as two points on the plane, and the line connecting the two points is divided into multiple segments according to a fixed value, and the segmentation point is the sampling point.

[0026] In this embodiment, the height sampling points are calculated by interpolation according to the target position and the position of the drone hangar. The target position and the position of the drone hangar can be regarded as two points on the plane. The line connecting the two points is divided into multiple segments according to a fixed value. The segmentation point is the sampling point, and this fixed value is the sampling distance. Among them, the drone hangar position and the target point will also be used as the starting and ending points of the sampling points.

[0027] In another embodiment, the specific calculation method of the sampling points is:

[0028] S101: First calculate the yaw angle from the position of the drone hangar to the target position;

[0029] S102: Acquire a sampling point at every sampling distance according to the direction of the yaw angle;

[0030] S103: until the distance between the latest sampling point and the target position is less than the sampling distance.

[0031] In this embodiment, the specific calculation method is as follows: first calculate the yaw angle from the library to the target point, and obtain a sampling point every sampling distance (the distance is set during use, generally according to the accuracy of the terrain data) in the yaw angle direction. Until the distance between the latest sampling point and the target point is less than the sampling distance. Figure 2 shown.

[0032] Yaw angle and sampling point calculation: Yaw angle: In aviation, the yaw angle is the horizontal angle of the aircraft's nose relative to the north direction of the earth. The north direction is 0 degrees and clockwise is positive. For example, if the aircraft's nose is facing east, the yaw angle is 90 degrees.

[0033] The calculation of the yaw angle is a professional conventional formula, and the calculation process is as follows. The following formula can be used to calculate the yaw angle (azimuth, bearing) from point A to point B. Assume that the longitude and latitude of point A are (lat1, lon1), and the longitude and latitude of point B are (lat2, lon2), both expressed in decimal degrees. Use the formula:

[0034]

[0035] in:

[0036] and are the latitudes of point A and point B respectively (expressed in radians).

[0037] ,in and are the longitudes of point A and point B respectively (expressed in radians).

[0038] The results Convert to degrees and normalize to the range 0 to 360 degrees:

[0039]

[0040] The calculation of the sampling point is a professional conventional formula, and the calculation process is as follows. To calculate the latitude and longitude of point C obtained after moving a distance L from point A along a given yaw angle (azimuth), you can use dead reckoning. The following are the specific steps and formulas:

[0041] 1) The latitude and longitude of point A are (lat1, lon1), the distance is L (in kilometers), and the yaw angle is bearing (in degrees).

[0042] 2) Convert the latitude and longitude of point A from degrees to radians:

[0043]

[0044] 3) Convert the yaw angle from degrees to radians:

[0045]

[0046] 4) The average value of the radius R of the earth is 6371 kilometers.

[0047] 5) Calculate the latitude and longitude of point C (in radians) using the following formula:

[0048]

[0049] 6) Convert the latitude and longitude of point C from radians back to degrees:

[0050]

[0051] In another embodiment, the sampling distance is set according to the accuracy of the terrain data.

[0052] In another embodiment, the step S200 further includes the following steps:

[0053] S201: Query the ground height H of all sampling points dem , and then add a relative ground height H r Obtain the sampling point height H and compose a sampling point height list in order;

[0054] S202: Connect the sampling points in sequence, calculate the slope of each line segment, and obtain an array storing the slopes;

[0055] S203: loop through the array to obtain the height extreme value point and the slope maximum value point;

[0056] S204: The starting point, the obtained altitude extreme value point and slope maximum value point and the end point together form a pre-flight point list.

[0057] In this embodiment, pre-way points are obtained based on sampling points and terrain data. The sampling points are very dense, and the UAV's route does not require so many way points, so it is necessary to screen out the characteristic points as pre-way points. The calculation of pre-way points is mainly used to reduce the amount of calculation in the next step while reflecting the terrain characteristics to ensure flight safety.

[0058] Query the surface height (Hdem) of all sampling points, then add a relative ground height (Hr) to obtain the sampling point height (H = Hdem + Hr), and form a sampling point height list in order. Place the coordinates of the entire sampling point list in a coordinate system where the horizontal axis is the distance and the vertical axis is the height, such as Figure 3 As shown, the solid line is the terrain, and the dots are the sampling points after obtaining the height. The height is queried by querying the terrain data. The terrain data adopts national public data or NASA's public data, generally dem. The specific acquisition of dem and reading of the data are beyond the scope of discussion of this patent. Definition of dem: Digital elevation model (DEM) or digital surface model (DSM) is a 3D computer graphic representation used to represent terrain data, representing terrain or covering objects, usually referring to the terrain of planets, moons or asteroids. "Global DEM" refers to a discrete global grid. DEM is often used in geographic information systems (GIS) and is the most common basis for digitally produced topographic maps. Digital terrain model (DTM) refers specifically to the ground surface, while DEM and DSM may represent tree canopies or building roofs.

[0059] Connect the sampling points in order, calculate the slope of each line segment, and obtain an array containing the slopes. If there are n points, then there are n-1 line segments in total. Calculate the slope of each line segment (k = ΔH / ΔL. ΔH is the difference between the ordinates of the two points, that is, the height difference; ΔL is the difference between the abscissas of the two points, that is, the distance difference), and obtain a slope array slopes = [k1, k2, ..., kn-2, kn-1];

[0060] Loop through slopes and set the current sequence number to i.

[0061] Calculate the height extreme point and the slope maximum point.

[0062] The starting point, all extreme value coordinates and the target point together form a list of pre-travel points, such as Figure 6 As shown, the solid line is the terrain, and the points are the pre-navigation points after screening.

[0063] The purpose of calculating the pre-route points is to reduce the amount of calculation in the last step. In actual implementation, this step can be skipped and all sampling points can be directly used to calculate the final route.

[0064] H = H dem + H r , this H is equivalent to the height of the waypoint, and the drone is naturally flying above the ground. Figure 3 As shown, the ground height H of the collection point dem Add a relative ground height H r , all points will form a set above the terrain that is almost parallel to the terrain. For example: there is a sampling point a, the altitude of the ground at this point is 200, we add a relative ground height of 100, and get the height of point a (altitude) 300. If point a finally becomes a waypoint, the altitude of the drone when it flies to this point is 300, and the relative ground height is 100.

[0065] Acquiring the sampling point in step S100 only obtains the position of the sampling point, and step S200 is used to obtain the height of the sampling point.

[0066] The order in step S201 is the order of acquiring the sampling points in step S100, from the initial point to the target point.

[0067] In another embodiment, the height extreme value points include height maximum value points and height minimum value points.

[0068] In another embodiment, when the slope of adjacent line segments changes from positive to negative, the point where the two line segments intersect is a point of maximum height; when the slope of adjacent line segments changes from negative to positive, the point where the two line segments intersect is a point of minimum height.

[0069] In this embodiment, if Figure 4 As shown in the figure, the extreme value of the height is a very obvious feature point, indicating the local fluctuation. When the slope of adjacent line segments changes from positive to negative, the point where the two line segments intersect is the maximum point. Similarly, when the slope of adjacent line segments changes from negative to positive, the focus of the two line segments is the minimum. That is: when slopes[i-1]>0 and slopes[i]<=0, the sampling point with serial number i is the maximum point of height, and when slopes[i-1]<0 and slopes[i]>=0, the sampling point with serial number i is the minimum point of height.

[0070] In another embodiment, the slope maximum point is where the slope of the current line segment is greater than the slopes of the previous and next line segments.

[0071] In this embodiment, if Figure 5As shown in the figure, the maximum slope indicates the bulge in the terrain where the overall altitude rises or falls, which is a very important terrain feature. When the slope of the current line segment is greater than the slopes of the previous and next line segments, the maximum slope is found. That is, when slopes[i-1]>slopes[i] and slopes[i-1]>slopes[i-2], the sampling point with sequence number i is the maximum slope point.

[0072] In another embodiment, the step S300 further includes the following steps:

[0073] S301: placing the coordinates of the entire pre-waypoint list in a coordinate system where the horizontal coordinate is the distance and the vertical coordinate is the height, and determining the waypoints starting from the starting point;

[0074] S302: Calculate the slopes of the lines connecting the current point and all subsequent points respectively, and the point with the largest slope is the next waypoint;

[0075] S303: Repeat the traversal to obtain all waypoints, and then obtain the final route.

[0076] For this embodiment, the best flight path is calculated by placing the coordinates of the entire list of pre-waypoints in a coordinate system where the horizontal coordinate is the distance and the vertical coordinate is the height, and determining the waypoints starting from the first point. Note that the first and last points of the pre-waypoints must be the first and last points of the final waypoints, because they represent the hangar (drone take-off point) and the target point respectively.

[0077] Calculate the slope of the line connecting all the points after the current point, and the point with the largest slope is the next waypoint. Then repeat the traversal to get the final route.

[0078] Specific:

[0079] a) Start counting from i=0, there are n points in total, and repeat n-2 times. The pre-waypoint array is denoted as points, and points[0] is the first waypoint.

[0080] b) Calculate the slopes of the lines connecting points[i] and all the points behind it, and get a slope array [ki+1, ki+2, ..., kn-2, kn-1]. If the value of i is 0, it is equivalent to calculating the slopes of n-1 line segments. If the value of i is n-2, the slope array has only one value;

[0081] c) Find the maximum value kj=max[ki+1,ki+2,……,kn-2,kn-1] in the slope array, the count of the maximum value is j, and points[j] is the waypoint;

[0082] d) i = j, repeat step b) until i is equal to n-2 to complete the loop, and all waypoints are obtained, such as Figure 7shown.

[0083] Among them, the slope here is different from the slope calculated in obtaining pre-navigation points. The slope in obtaining pre-navigation points is the slope of the line connecting adjacent points calculated in sequence, that is, if there are points 1, 2, 3, 4, and 5, the calculated slopes are the slopes of line segment 12, line segment 23, line segment 34, and line segment 45 respectively. The slope here is the slope of the line connecting the i-th point and the points behind it, that is, points 1, 2, 3, 4, and 5. When i is 0, the calculated slopes are the slopes of line segment 12, line segment 13, line segment 14, and line segment 15 respectively; when i is 1, the calculated slopes are the slopes of line segment 23, line segment 24, and line segment 25 respectively.

[0084] e) Connect the waypoints in order to get the route trajectory, such as Figure 8 shown.

[0085] In short, the route is equivalent to a straight line to the target in the horizontal direction, avoiding detours. All sampling points are below the route, ensuring that the drone will not hit the mountain when flying on this route, ensuring safety. The route waypoints are minimized to make the drone flight trajectory simpler, which not only reduces the distance the drone flies, but also allows the drone to maintain the fastest flight speed.

[0086] If the drone directly takes off to the maximum altitude of the sampling point, flies horizontally and then lowers its altitude to above the target, the flight distance will be longer. In addition, the drone's vertical up and down flight consumes a lot of power and has a very slow flight speed.

[0087] The point with the largest slope is chosen as the next waypoint because it means that the line connecting the two points must be above other points "after the current point". This ensures the route, the route trajectory is simple, the drone flies faster and safer, and will not hit the mountain.

[0088] Finally, it should be noted that, under the guidance of this specification and without departing from the scope of protection of the claims of the present invention, ordinary technicians in this field can also make many kinds of changes in form, which are all protected by the present invention.

Claims

1. A method for automatic emergency route planning of a drone, the method comprising the following steps: S100: Sampling between the drone hangar and the target according to the terrain, obtaining a large number of sampling points, among which, The drone hangar location and target location are used as the starting and ending points of the sampling points; S200: obtaining a large number of pre-navigation points according to the sampling points and terrain data; S300: Optimizing the pre-waypoints to obtain waypoints, and connecting the waypoints in sequence to obtain a route track.

2. According to the method as claimed in claim 1, preferably, the sampling points are obtained according to the following steps: the target position and the position of the drone hangar are regarded as two points on the plane, and the line connecting the two points is divided into multiple segments according to a fixed value, and the segmentation point is the sampling point.

3. The method according to claim 2, wherein the specific calculation method of the sampling points is: S101: First calculate the yaw angle from the position of the drone hangar to the target position; S102: Acquire a sampling point at every sampling distance according to the direction of the yaw angle; S103: until the distance between the latest sampling point and the target position is less than the sampling distance. The method according to claim 3 , wherein the sampling distance is set according to the accuracy of the terrain data.

5. The method according to claim 1, wherein step S200 further comprises the following steps: S201: Query the ground height H of all sampling points dem , and then add a relative ground height H r Obtain the sampling point height H and compose a sampling point height list in order; S202: Connect the sampling points in sequence, calculate the slope of each line segment, and obtain an array storing the slopes; S203: loop through the array to obtain the height extreme value point and the slope maximum value point; S204: The starting point, the obtained altitude extreme value point and slope maximum value point and the end point together form a pre-flight point list.

6. The method according to claim 5, wherein the height extreme value points include height maximum value points and height minimum value points.

7. The method of claim 6, wherein: When the slope of adjacent line segments changes from positive to negative, the point where the two line segments intersect is the point of maximum height; when the slope of adjacent line segments changes from negative to positive, the point where the two line segments intersect is the point of minimum height.

8. The method as claimed in claim 5, wherein the slope maximum point is where the slope of the current line segment is greater than the slopes of the previous and next line segments.

9. The method according to claim 1, wherein step S300 further comprises the following steps: S301: placing the coordinates of the entire pre-waypoint list in a coordinate system where the horizontal coordinate is the distance and the vertical coordinate is the height, and determining the waypoints starting from the starting point; S302: Calculate the slopes of the lines connecting the current point and all subsequent points respectively, and the point with the largest slope is the next waypoint; S303: Repeat the traversal to obtain all waypoints, and then obtain the final route.

Images