A UAV trajectory planning method and system for bird tracking

Abstract

The present invention relates to the field of unmanned aerial vehicle path planning, and specifically to a method and system for unmanned aerial vehicle trajectory planning for bird tracking; a first predicted trajectory is first obtained according to first position data, a second predicted trajectory is then planned according to first instruction data and the first predicted trajectory, and a straight trajectory is calculated according to the movement speed of the unmanned aerial vehicle and the first instruction data; when the distance between the straight trajectory and the bird is less than the avoidance radius, a first distance difference is calculated and a calculation formula is reconstructed to obtain a second moment, and a curved trajectory avoiding an avoidance cylinder is obtained; an avoidance rotating body is then constructed for tree obstacles, and a portion of the first trajectory is projected onto the avoidance rotating body to extract the projection curve to obtain a second trajectory, thereby completing the trajectory planning of the unmanned aerial vehicle when tracking birds; the problem that the prior art may touch shaking branches or flying birds when performing close-up tracking of birds, thereby causing bird injuries or damage to the unmanned aerial vehicle is solved.

Description

Technical Field

[0001] The present invention relates to the field of unmanned aerial vehicle (UAV) path planning, and in particular to a UAV trajectory planning method and system for bird tracking. Background Art

[0002] In mountain forest nature reserves, drones are needed to patrol the mountain forest areas to detect whether there are fire spots, abnormal people or new birds. For new or existing birds in mountain forest nature reserves, it is sometimes necessary to photograph them with drones to understand their habits, etc., in order to collect information or conduct research.

[0003] However, when birds are flying and it is necessary to shoot them at close range to understand their different flying postures, the drone may touch shaking branches or flying birds during obstacle avoidance, causing damage to the drone. For example, when the drone frequently changes positions around the birds to change the shooting angle, the drone may just touch flying birds or branches blown by the wind when it moves from the current position to the next position according to instructions, causing damage to the drone or injury to the bird. Summary of the invention

[0004] In view of the shortcomings of the prior art, the present invention provides a UAV trajectory planning method and system for bird tracking, which solves the problem that the prior art may touch swaying branches or flying birds when performing close-up tracking of birds, causing bird injuries or damage to the UAV.

[0005] To achieve the above object, the present invention provides the following technical solutions:

[0006] A UAV trajectory planning method for bird tracking, the method comprising:

[0007] S1, obtaining a topographic map, and continuously obtaining first position data of coordinates of the bird to be tracked and the drone on the topographic map, and second position data of coordinates of tree obstacles located on the flight path of the bird and the drone;

[0008] S2, obtaining first instruction data of the inputted position of the UAV relative to the bird, and planning a first trajectory of the UAV to avoid the bird according to the first position data;

[0009] S3, judging whether there is an obstacle at the first track according to the topographic map and the second position data;

[0010] If yes, proceed to step S4;

[0011] If not, proceed to step S5;

[0012] S4, establishing a minimum rotating body and an avoidance rotating body including the tree obstacle according to the second position data, and planning the first trajectory to obtain a second trajectory avoiding the tree obstacle;

[0013] S5. Determine whether the drone has reached the predetermined position according to the first instruction data and the first position data;

[0014] If so, then end;

[0015] If not, return to step S2.

[0016] Preferably, in step S2, the following steps are specifically included:

[0017] S21, obtaining first instruction data and first position data of the inputted position of the drone relative to the bird;

[0018] S22, setting an avoidance radius for monitoring birds, constructing a first obstacle circle with the coordinates of the location of the birds as the center of the sphere, and adding the first obstacle circle to the topographic map;

[0019] S23, calculating the moving speed and direction of the bird at any moment according to the first position data, and predicting a first predicted flight trajectory of the bird;

[0020] S24, establishing a second predicted trajectory of the UAV that satisfies the first instruction data according to the first predicted trajectory; the coordinates of any position point on the second predicted trajectory satisfy the following relationship:

[0021]

[0022] In the above formula, represents the relative position of the bird to which the UAV needs to reach contained in the first instruction data and the vector of the bird's position, and represents the X-axis, Y-axis, and Z-axis coordinates of the position point at the i-th moment on the second predicted trajectory, and Respectively represent the X-axis, Y-axis and Z-axis coordinates of the position point of the bird at the i-th moment on the first predicted trajectory;

[0023] S25, determining whether the current relative positions of the drone and the bird satisfy the first instruction data;

[0024] If yes, then the second predicted trajectory is output and the process proceeds to step S29;

[0025] If not, proceed to step S26;

[0026] S26, planning the drone to move to a straight line trajectory that satisfies the position in the first instruction data according to the first predicted trajectory, the second predicted trajectory, the first position data and the current speed of the drone;

[0027] S27, calculating the Euclidean distance between the UAV and the bird at any time on the straight line trajectory, marking it as the first distance, and determining whether the first distance with the smallest value is less than the avoidance radius;

[0028] If yes, proceed to step S28;

[0029] If not, the straight line trajectory is output and the process goes to step S29;

[0030] S28, establishing an avoidance cylinder with the first predicted trajectory as the central axis according to the first obstacle circle, and correcting the straight trajectory to obtain a curved trajectory that avoids the avoidance cylinder;

[0031] S29: Mark the second predicted trajectory, the straight trajectory, and the curved trajectory as the first trajectory.

[0032] Preferably, in step S23, the following steps are specifically included:

[0033] S231, calculating the moving speed of the bird at any time according to the first position data; the calculation formula of the moving speed is:

[0034]

[0035] In the above formula, V i is the moving speed of the bird at any moment, Δt is the time interval between two adjacent moments, ΔS i is the time between any one moment and two adjacent moments for the bird;

[0036] S232, obtaining the coordinates of the first bird position point within a certain time interval from the current time;

[0037] S233, connecting the first bird position points in sequence according to the time sequence, and calculating the deflection angle of any first bird position point according to the first vector of the line between any first bird position point and two first bird position points connected to it;

[0038] S234, setting a deflection angle threshold, and marking the first bird position point whose deflection angle is greater than the deflection angle threshold as a turning point;

[0039] S235, obtaining the turning point closest to the current moment, and obtaining all first bird position points from the turning point to the current moment, and marking them as second bird position points;

[0040] S236, calculating the second vector from the second bird position point at any moment to the second bird position point at the next moment in chronological order; the calculation formula of the second vector is:

[0041]

[0042] In the above formula, The second vector representing the distance from the second bird position at any moment to the second bird position at the next moment, x i+1 ,y i+1 and z i+1 are the X-axis, Y-axis, and Z-axis coordinates of the second bird's position at the next moment, respectively. i ,y i and z i are the X-axis, Y-axis and Z-axis coordinates of the second bird position point at the i-th moment;

[0043] S237, calculating the sum of the second vectors to obtain a third vector, and establishing a first predicted trajectory extending from the first bird position point at the current moment along the direction of the third vector; the calculation formula of the third vector is:

[0044]

[0045] In the above formula, represents the third vector, represents the second vector, and n represents the number of the second vector.

[0046] Preferably, in step S26, the following steps are specifically included:

[0047] S261, according to the first predicted trajectory, the second predicted trajectory and the speed of the drone, calculating the first moment when the drone reaches the position on the second predicted trajectory that satisfies the first instruction data; the first moment satisfies the following relationship:

[0048]

[0049] In the above formula, x 0 ,y 0 and z 0 They represent the X-axis, Y-axis, and Z-axis coordinates of the drone at the current moment, k represents the kth moment from the current moment, Δt is the time interval between two adjacent moments, and V0 represents the flight speed set by the drone;

[0050] S262, obtaining the first designated position coordinates of the UAV at the first moment according to the second predicted trajectory;

[0051] S263: Connect the coordinates of the first designated position and the coordinates of the drone at the current moment to obtain a straight line trajectory.

[0052] Preferably, in step S28, the following steps are specifically included:

[0053] S281, establishing an avoidance cylinder with the first predicted trajectory as the central axis, wherein the radius of the avoidance cylinder is equal to the avoidance radius of the first obstacle circle;

[0054] S282, establishing a first dividing plane parallel to the first predicted trajectory on the straight trajectory, dividing the avoidance cylinder into two halves through the first dividing plane, and extracting the avoidance cylinder with a smaller arc length of the cross section and marking it as a projection cylinder;

[0055] S283, establishing a second dividing plane perpendicular to the first dividing plane on the straight line trajectory, and obtaining a dividing line of the second dividing plane on the projected cylindrical surface;

[0056] S284, obtaining the length of the curve and the length of the straight line in the dividing line, and calculating the difference between the two to obtain a first distance difference; the calculation formula of the first distance difference is:

[0057] ΔS 0 =S C -S L

[0058] In the above formula, ΔS 0 Indicates the first distance difference between the length of the curve and the length of the straight line in the dividing line, S C Indicates the length of the curve, S L Indicates the length of a straight line;

[0059] S285, calculating a second time when the drone reaches a position on the second predicted trajectory that satisfies the first instruction data according to the first distance difference, the first predicted trajectory, the second predicted trajectory, and the speed of the drone;

[0060] S286. Obtain the coordinates of the designated position of the UAV at the second moment according to the second predicted trajectory, and establish the shortest path through the projection cylindrical surface according to the coordinates and the coordinates of the current position of the UAV to generate a curved trajectory.

[0061] Preferably, in step S286, the following steps are specifically included:

[0062] S2861, obtaining the second designated position coordinates of the UAV at the second moment according to the second predicted trajectory;

[0063] S2862, connecting the coordinates of the second designated position and the coordinates of the drone at the current moment to obtain a straight line trajectory;

[0064] S2863, obtaining the curve portion corresponding to the dividing line on the projection cylindrical surface according to the straight line trajectory;

[0065] S2864, establishing a first tangent line between the second designated position coordinates and the dividing line curve portion, and obtaining a first tangent point;

[0066] S2865, establishing a second tangent line between the current position of the drone and the curve portion of the dividing line, and obtaining a second tangent point;

[0067] S2866, connect the arc line of the first tangent point and the second tangent point on the projected cylindrical surface, the first tangent line and the second tangent line to establish a curve trajectory.

[0068] Preferably, in step S4, the following steps are specifically included:

[0069] S41. Setting a safety radius for avoiding tree obstacles;

[0070] S42, obtaining coordinate values ​​of the tree obstacle at several moments according to the second position data;

[0071] S43, establishing a minimum rotation body including the coordinates of the tree obstacle;

[0072] S44, increasing the cross-sectional radius of the minimum rotating body to obtain an avoidance rotating body;

[0073] S45, projecting the portion of the first trajectory passing through the avoidance rotating body onto the surface of the avoidance rotating body along the cross-sectional direction, and extracting the projection curve with the shortest arc length to obtain the avoidance trajectory;

[0074] S46: Connect the avoidance trajectory and the portion of the first trajectory that does not correspond to the avoidance trajectory to obtain a second trajectory.

[0075] Preferably, in step S233, the calculation formula of the deflection angle is:

[0076]

[0077] In the above formula, θ i is the deflection angle of the first bird position point at the i-th moment, Represents the first vector from the first bird position point at the i-th moment to the first bird position point at the next adjacent moment, Represents the first vector from the first bird position point at the i-th moment to the first bird position point at the previous adjacent moment.

[0078] Preferably, in step S285, the second moment satisfies the following relationship:

[0079]

[0080] In the above formula, It represents the first distance difference corresponding to the time when the position in the first instruction data is reached at the i-th moment.

[0081] A planning system for implementing a drone trajectory planning method comprises a processor and a memory, wherein the memory is used to store a computer program, and the computer program implements the drone trajectory planning method when executed by the processor.

[0082] Compared with the prior art, the present invention provides a UAV trajectory planning method and system for bird tracking, which has the following beneficial effects:

[0083] 1. The present invention obtains a first predicted trajectory according to first position data, then plans a second predicted trajectory according to the first instruction data and the first predicted trajectory, and then calculates a straight trajectory according to the moving speed of the UAV and the first instruction data. Then, when the distance between the straight trajectory and the bird is less than the avoidance radius, the first distance difference is calculated and the calculation formula is reconstructed to obtain the second moment, and a curved trajectory avoiding the avoidance cylinder is obtained. Then, according to the characteristics of tree obstacles, an avoidance rotor is constructed, and a part of the first trajectory is projected onto the avoidance rotor through the path on the avoidance rotor to extract the projection curve with the shortest arc length, and then the second trajectory can be obtained, thereby completing the trajectory planning of the UAV when tracking birds.

[0084] 2. The present invention constructs an avoidance cylinder when the first distance is less than the avoidance radius, thereby eliminating the need to consider that when the speed of the bird changes, the curved trajectory needs to be frequently and significantly corrected, thereby saving calculation complexity and making the curved trajectory planning of the UAV faster.

[0085] 3. The present invention calculates the first distance difference, that is, calculates the distance difference between the portion of the straight line trajectory passing through the avoidance cylinder and the curved portion projected onto the avoidance cylinder, so that a calculation equation can be further constructed based on this to calculate the second moment, so that the straight line trajectory can be replanned according to the second moment and a curved trajectory that meets the position requirement of the first instruction data and reaches the corresponding position as quickly as possible can be constructed.

[0086] 4. The present invention constructs an avoidance rotating body to prevent branches from swinging under the action of external force and colliding with the drone. At the same time, a projection curve is constructed to serve as the shortest path for the drone to cross the avoidance rotating body, that is, the avoidance trajectory, so that the distance of the generated second trajectory is the shortest, thereby preventing the drone from being lost. BRIEF DESCRIPTION OF THE DRAWINGS

[0087] The drawings described herein are used to provide a further understanding of the present application and constitute a part of the present application. The illustrative embodiments of the present application and their descriptions are used to explain the present application and do not constitute an improper limitation on the present application. In the drawings:

[0088] Figure 1 It is a flow chart of the UAV trajectory planning method of the present invention;

[0089] Figure 2 is a schematic diagram of a straight line trajectory of the UAV of the present invention;

[0090] Figure 3 A schematic diagram of a cylinder avoidance method according to the present invention;

[0091] Figure 4 Schematic diagram of the curve trajectory of the present invention. DETAILED DESCRIPTION

[0092] In order to make the above-mentioned purposes, features and advantages of the present invention more obvious and easy to understand, the present invention is further described in detail below with reference to the accompanying drawings and specific implementation methods, so that the implementation process of how the present application uses technical means to solve technical problems and achieve technical effects can be fully understood and implemented accordingly.

[0093] Those skilled in the art can understand that all or part of the steps in the following embodiments can be completed by instructing the relevant hardware through a program, so the present application can be in the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware. Moreover, the present application can be in the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program codes.

[0094] In order to solve the problem that the existing technology may touch the shaking branches or flying birds when tracking birds at close range, causing bird injuries or damage to the drone, the present invention provides a drone trajectory planning method for bird tracking, such as Figure 1 As shown, the method comprises the following steps:

[0095] S1. Obtain a topographic map, and continuously obtain first position data of coordinates of the bird to be tracked and the drone on the topographic map, and second position data of coordinates of tree obstacles located on the flight path of the bird and the drone; there are many ways to obtain data, such as through radar or images, and the topographic map can be obtained and established by GIS or digital sand table;

[0096] S2, obtaining the first instruction data of the inputted position of the drone relative to the bird, and planning the first trajectory of the drone to avoid the bird according to the first position data; the first instruction data is an instruction inputted by the user for controlling the drone to move to a certain distance and angle corresponding to the bird, which can be a preset default instruction or a real-time input instruction. During the movement of the drone, the first trajectory for avoiding the flying bird is first planned, such as Figure 2 As shown, in step S2, the following steps are specifically included:

[0097] S21, obtaining first instruction data and first position data of the inputted position of the drone relative to the bird;

[0098] S22. Setting an avoidance radius for monitoring birds, constructing a first obstacle circle with the coordinates of the bird's location as the center, and adding the first obstacle circle to the topographic map; the UAV is always outside the first obstacle circle during movement to avoid hitting or disturbing the birds due to positioning errors of the UAV.

[0099] S23, calculating the moving speed and direction of the bird at any time according to the first position data, and predicting the first predicted trajectory of the bird's flight; when the UAV finds the bird, it may be far away from the bird and does not meet the position requirement of the first instruction data, so it needs to fly to the specified position of the first instruction data, but when the bird is flying, it is necessary to predict the flight trajectory of the bird. The UAV needs to fly to the flight trajectory in front of the current position of the bird to approach the bird as quickly as possible. At this time, it is necessary to make a prediction based on the first position data, but it is necessary to identify the valid data that can be used for prediction to improve the accuracy of the first trajectory. Therefore, in step S23, the following steps are specifically included:

[0100] S231, calculating the moving speed of the bird at any time according to the first position data; the calculation formula of the moving speed is:

[0101]

[0102] In the above formula, V i is the moving speed of the bird at any moment, Δt is the time interval between two adjacent moments, ΔS i is the time between any one moment and two adjacent moments for the bird;

[0103] S232, obtaining the coordinates of the first bird position point within a certain time interval from the current time;

[0104] S233, connecting the first bird position points in sequence in time order, and calculating the deflection angle of any first bird position point according to the first vector of the line between any first bird position point and two first bird position points connected to it; the calculation formula of the deflection angle is:

[0105]

[0106] In the above formula, θ i is the deflection angle of the first bird position point at the i-th moment, Represents the first vector from the first bird position point at the i-th moment to the first bird position point at the next adjacent moment, represents the first vector from the first bird position point at the ith moment to the first bird position point at the previous moment adjacent to it;

[0107] S234, setting a deflection angle threshold, and marking the first bird position point whose deflection angle is greater than the deflection angle threshold as a turning point;

[0108] S235, obtaining the turning point closest to the current moment, and obtaining all the first bird position points from the turning point to the current moment, and marking them as second bird position points; thereby finding the position where the bird turns during flight, and using the position as the screening basis for the coordinate data of the first bird position point, here the coordinates of the first bird position point between the turning point closest to the current moment and the current moment are selected as the second bird position point, the bird basically flies in a straight line at the moment corresponding to the second bird position point, and therefore the first predicted trajectory of the bird is inferred based on the position coordinates of the second bird position point;

[0109] S236, calculating the second vector from the second bird position point at any moment to the second bird position point at the next moment in chronological order; the calculation formula of the second vector is:

[0110]

[0111] In the above formula, The second vector representing the distance from the second bird position at any moment to the second bird position at the next moment, x i+1 ,y i+1 and z i+1 are the X-axis, Y-axis, and Z-axis coordinates of the second bird's position at the next moment, respectively. i ,y i and z i are the X-axis, Y-axis and Z-axis coordinates of the second bird position point at the i-th moment;

[0112] S237, calculating the sum of the second vectors to obtain a third vector, and establishing a first predicted trajectory extending from the first bird position point at the current moment along the direction of the third vector; the calculation formula of the third vector is:

[0113]

[0114] In the above formula, represents the third vector, represents the second vector, and n represents the number of the second vectors. The bird may also experience slight fluctuations in position during its straight-line flight. Therefore, the third vector is used to comprehensively reflect the wind direction of the bird during the time corresponding to the second bird position point. Thus, the first predicted trajectory as a straight line can be obtained according to the first bird position point at the current moment and the third vector.

[0115] S24, establishing a second predicted trajectory of the UAV that satisfies the first instruction data according to the first predicted trajectory; the coordinates of any position point on the second predicted trajectory satisfy the following relationship:

[0116]

[0117] In the above formula, The vector representing the relative position of the drone to the bird and the bird's position contained in the first instruction data reflects the relative angle and distance between the drone and the bird. and represents the X-axis, Y-axis, and Z-axis coordinates of the position point at the i-th moment on the second predicted trajectory, and Respectively represent the X-axis, Y-axis and Z-axis coordinates of the position point of the bird at the i-th moment on the first predicted trajectory;

[0118] S25, determining whether the current relative positions of the drone and the bird satisfy the first instruction data;

[0119] If yes, then the second predicted trajectory is output and the process proceeds to step S29;

[0120] If not, proceed to step S26;

[0121] S26, planning the drone to move to a straight line trajectory that satisfies the position in the first instruction data according to the first predicted trajectory, the second predicted trajectory, the first position data and the current speed of the drone; determining a straight line between two points, which can enable the drone to fly to the position that satisfies the first instruction data as quickly as possible to prevent the bird from being lost, and the position is located on the second predicted trajectory, so it is necessary to calculate where the position is located on the second predicted trajectory, and in step S26, the following steps are specifically included:

[0122] S261, according to the first predicted trajectory, the second predicted trajectory and the speed of the drone, calculating the first moment when the drone reaches the position on the second predicted trajectory that satisfies the first instruction data; the first moment satisfies the following relationship:

[0123]

[0124] In the above formula, x 0 ,y 0 and z 0 They represent the X-axis, Y-axis, and Z-axis coordinates of the drone at the current moment, k represents the kth moment from the current moment, Δt is the time interval between two adjacent moments, and V0 represents the flight speed set by the drone. The speed at this time is generally set to the maximum speed allowed by the drone to reach the designated location as quickly as possible;

[0125] S262, obtaining the first designated position coordinates of the drone at the first moment according to the second predicted trajectory; that is, bringing the first moment to the corresponding moment in the second predicted trajectory to obtain the corresponding first designated position coordinates;

[0126] S263: Connect the coordinates of the first designated position and the coordinates of the drone at the current moment to obtain a straight line trajectory.

[0127] S27, calculating the Euclidean distance between the UAV and the bird at any time on the straight track, marking it as the first distance, and determining whether the first distance with the smallest value is less than the avoidance radius, so as to determine whether the bird will enter the first obstacle circle when flying along the straight track. When there is a part of the straight track that enters the first obstacle circle, the straight track needs to be corrected later to prevent the UAV from entering the first obstacle circle;

[0128] If yes, proceed to step S28;

[0129] If not, the straight line trajectory is output and the process goes to step S29;

[0130] S28, establishing an avoidance cylinder with the first predicted trajectory as the central axis according to the first obstacle circle, and correcting the straight trajectory to obtain a curved trajectory that avoids the avoidance cylinder; Figure 3 and Figure 4 As shown in the figure, when there is a portion of the straight track that enters the first obstacle circle, it means that the positions of the bird and the drone are relatively close in this portion. At this time, if the curved track is made to fit on the first obstacle circle, the acceleration or deceleration of the bird may cause the curved track to fluctuate frequently. Therefore, the straight track is corrected by establishing an avoidance cylinder to obtain the curved track. Therefore, in step S27, the minimum distance between the straight track and the first predicted track can be calculated as the first distance for judgment. In this step S28, in order to further illustrate the generation method of the avoidance cylinder and the curved track, in step S28, the following steps are specifically included:

[0131] S281, establishing an avoidance cylinder with the first predicted trajectory as the central axis, wherein the radius of the avoidance cylinder is equal to the avoidance radius of the first obstacle circle;

[0132] S282, establishing a first dividing plane parallel to the first predicted trajectory on the straight trajectory, dividing the avoidance cylinder into two halves through the first dividing plane, and extracting the avoidance cylinder with a smaller arc length of the cross section and marking it as a projection cylinder;

[0133] S283, establishing a second dividing plane perpendicular to the first dividing plane on the straight line trajectory, and obtaining a dividing line of the second dividing plane on the projected cylindrical surface;

[0134] S284, obtaining the length of the curve and the length of the straight line in the dividing line, and calculating the difference between the two to obtain a first distance difference; the calculation formula of the first distance difference is:

[0135] ΔS 0 =S C -S L

[0136] In the above formula, ΔS 0 Indicates the first distance difference between the length of the curve and the length of the straight line in the dividing line, S C Indicates the length of the curve, S L Indicates the length of a straight line;

[0137] S285, calculating the second moment when the drone reaches the position in the first instruction data on the second predicted trajectory according to the first distance difference, the first predicted trajectory, the second predicted trajectory and the speed of the drone; the second moment satisfies the following relationship:

[0138]

[0139] In the above formula, Indicates the first distance difference corresponding to the time when the position in the first instruction data is reached at the i-th moment;

[0140] S286, according to the second predicted trajectory, the coordinates of the designated position of the UAV at the second moment are obtained, and the shortest path through the surface of the projection cylinder is established according to the coordinates and the coordinates of the current position of the UAV to generate a curved trajectory. The shortest path is the straight line trajectory obtained first, and then the part of the straight line trajectory passing through the projection cylinder is projected onto its cylindrical surface, so that the UAV avoids the birds. Therefore, in step S286, the following steps are specifically included:

[0141] S2861, obtaining the second designated position coordinates of the UAV at the second moment according to the second predicted trajectory;

[0142] S2862, connecting the coordinates of the second designated position and the coordinates of the drone at the current moment to obtain a straight line trajectory;

[0143] S2863, obtaining the curve portion corresponding to the dividing line on the projection cylindrical surface according to the straight line trajectory;

[0144] S2864, establishing a first tangent line between the second designated position coordinates and the dividing line curve portion, and obtaining a first tangent point;

[0145] S2865, establishing a second tangent line between the current position of the drone and the curve portion of the dividing line, and obtaining a second tangent point;

[0146] S2866. Connect the arc of the first tangent point and the second tangent point on the projected cylindrical surface, the first tangent line and the second tangent line to establish a curved trajectory. When the UAV flies along the curved trajectory at the calculated speed V0, since the distance of the curved trajectory is less than the total length of the curved part and the part of the straight trajectory that does not correspond to the curved trajectory, the UAV will arrive at the corresponding position before the bird. If the UAV is to fly at the maximum speed, the speed of the UAV at the time of calculation can be increased by a certain amount according to its maximum flight speed, and then the actual flight speed within the upper limit of the UAV flight is calculated by calculating the length of the curved trajectory and the second moment. Of course, the speed of the UAV can also be not increased during the calculation, and the calculated actual flight speed can be directly used as the speed of the UAV to fly to the specified position.

[0147] S29: Mark the second predicted trajectory, the straight trajectory, and the curved trajectory as the first trajectory.

[0148] S3, judging whether there is an obstacle at the first track according to the topographic map and the second position data;

[0149] If yes, proceed to step S4;

[0150] If not, proceed to step S5;

[0151] S4, establishing a minimum rotating body and an avoidance rotating body including the tree obstacle according to the second position data, and planning the first trajectory to obtain a second trajectory avoiding the tree obstacle; because the tree branches will swing under the blowing of wind, taking the tree branches as reference points when they are stationary, they will swing within a certain range around the range, so the swinging range is generally a range where a rotating body is located. In order to further obtain the range of the rotating body, in step S4, the following steps are specifically included:

[0152] S41. Setting a safety radius for avoiding tree obstacles;

[0153] S42, obtaining coordinate values ​​of the tree obstacle at several moments according to the second position data;

[0154] S43, establishing a minimum rotation body including the coordinates of the tree obstacle;

[0155] S44, increasing the cross-sectional radius of the minimum rotating body to obtain an avoidance rotating body;

[0156] S45, projecting the portion of the first trajectory passing through the avoidance rotating body onto the surface of the avoidance rotating body along the cross-sectional direction, and extracting the projection curve with the shortest arc length to obtain the avoidance trajectory;

[0157] S46. Connect the avoidance trajectory and the portion of the first trajectory that does not correspond to the avoidance trajectory to obtain a second trajectory. The projection curve with the shortest arc length is used as the shortest path for the UAV to cross the avoidance rotating body, that is, the avoidance trajectory. This makes the generated second trajectory have the shortest distance, thereby preventing the UAV from losing track of the bird and saving power.

[0158] S5. Determine whether the drone has reached the predetermined position according to the first instruction data and the first position data;

[0159] If so, then end;

[0160] If not, return to step S2.

[0161] In view of the branches that may be encountered by the drone in the process of tracking birds, and the situation that the birds may be encountered or disturbed in the process of changing the position to shoot bird images at different angles, the present invention continuously obtains the second position data of the birds, the drone and the identified tree obstacles in the process of tracking the birds, first obtains the first predicted trajectory according to the first position data, plans the second predicted trajectory according to the first instruction data and the first predicted trajectory, and then calculates the straight line trajectory according to the movement speed of the drone and the first instruction data, and then reconstructs the calculation by further calculating the first distance difference when the distance between the straight line trajectory and the bird is less than the avoidance radius. The calculation formula of the second moment is calculated, and then the coordinate point on the second predicted trajectory at the second moment is connected with the current position point of the UAV to regain the straight trajectory, and then the curved trajectory is finally obtained by projecting part of the trajectory onto the avoidance cylinder. Then, according to the characteristics of the tree obstacle, an avoidance rotor is constructed, and the path of part of the first trajectory passing through the avoidance rotor is projected onto the avoidance rotor to extract the projection curve with the shortest arc length, and then the second trajectory can be obtained, and the first trajectory and the second trajectory are continuously updated by periodically detecting whether the relative position of the UAV and the bird meets the first instruction data.

[0162] Corresponding to the UAV trajectory planning method for bird tracking provided in the above-mentioned embodiment, the present invention also provides a UAV trajectory planning system for bird tracking, including a processor and a memory, the memory is used to store a computer program, and when the computer program is executed by the processor, the UAV trajectory planning method for bird tracking is implemented.

[0163] The above implementation methods have been described in detail. Specific examples are used herein to illustrate the principles and implementation methods of the present invention. The description of the above embodiments is only used to help understand the method of the present invention and its core idea. At the same time, for those skilled in the art, according to the idea of ​​the present invention, there will be changes in the specific implementation methods and application scope. In summary, the content of this specification should not be understood as a limitation on the present invention.

Claims

1. A UAV trajectory planning method for bird tracking, characterized in that: The method includes: S1, obtaining a topographic map, and continuously obtaining first position data of coordinates of the bird to be tracked and the drone on the topographic map, and second position data of coordinates of tree obstacles located on the flight path of the bird and the drone; S2, obtaining first instruction data of the inputted position of the UAV relative to the bird, and planning a first trajectory of the UAV to avoid the bird according to the first position data; In step S2, the following steps are specifically included: S21, obtaining first instruction data and first position data of the inputted position of the drone relative to the bird; S22, setting an avoidance radius for monitoring birds, constructing a first obstacle circle with the coordinates of the location of the birds as the center of the sphere, and adding the first obstacle circle to the topographic map; S23, calculating the moving speed and direction of the bird at any moment according to the first position data, and predicting a first predicted flight trajectory of the bird; S24, establishing a second predicted trajectory of the UAV that satisfies the first instruction data according to the first predicted trajectory; the coordinates of any position point on the second predicted trajectory satisfy the following relationship: In the above formula, represents the relative position of the bird to which the UAV needs to reach contained in the first instruction data and the vector of the bird's position, and represents the X-axis, Y-axis, and Z-axis coordinates of the position point at the i-th moment on the second predicted trajectory, and Respectively represent the X-axis, Y-axis and Z-axis coordinates of the position point of the bird at the i-th moment on the first predicted trajectory; S25, determining whether the current relative positions of the drone and the bird satisfy the first instruction data; If yes, then the second predicted trajectory is output and the process proceeds to step S29; If not, proceed to step S26; S26, planning the drone to move to a straight line trajectory that satisfies the position in the first instruction data according to the first predicted trajectory, the second predicted trajectory, the first position data and the current speed of the drone; S27, calculating the Euclidean distance between the UAV and the bird at any time on the straight line trajectory, marking it as the first distance, and determining whether the first distance with the smallest value is less than the avoidance radius; If yes, proceed to step S28; If not, the straight line trajectory is output and the process goes to step S29; S28, establishing an avoidance cylinder with the first predicted trajectory as the central axis according to the first obstacle circle, and correcting the straight trajectory to obtain a curved trajectory that avoids the avoidance cylinder; S29, marking the second predicted trajectory, the straight trajectory, and the curved trajectory as the first trajectory; S3, judging whether there is an obstacle at the first track according to the topographic map and the second position data; If yes, proceed to step S4; If not, proceed to step S5; S4, establishing a minimum rotating body and an avoidance rotating body including the tree obstacle according to the second position data, and planning the first trajectory to obtain a second trajectory avoiding the tree obstacle; In step S4, the following steps are specifically included: S41. Setting a safety radius for avoiding tree obstacles; S42, obtaining coordinate values ​​of the tree obstacle at several moments according to the second position data; S43, establishing a minimum rotation body including the coordinates of the tree obstacle; S44, increasing the cross-sectional radius of the minimum rotating body to obtain an avoidance rotating body; S45, projecting the portion of the first trajectory passing through the avoidance rotating body onto the surface of the avoidance rotating body along the cross-sectional direction, and extracting the projection curve with the shortest arc length to obtain the avoidance trajectory; S46, connecting the avoidance trajectory and a portion of the first trajectory that does not correspond to the avoidance trajectory to obtain a second trajectory; S5. Determine whether the drone has reached the predetermined position according to the first instruction data and the first position data; If so, then end; If not, return to step S2.

2. The planning method according to claim 1, characterized in that: In step S23, the following steps are specifically included: S231, calculating the moving speed of the bird at any time according to the first position data; the calculation formula of the moving speed is: In the above formula, V i is the moving speed of the bird at any moment, Δt is the time interval between two adjacent moments, ΔS i is the time between any one moment and two adjacent moments for the bird; S232, obtaining the coordinates of the first bird position point within a certain time interval from the current time; S233, connecting the first bird position points in sequence according to the time sequence, and calculating the deflection angle of any first bird position point according to the first vector of the line between any first bird position point and two first bird position points connected to it; S234, setting a deflection angle threshold, and marking the first bird position point whose deflection angle is greater than the deflection angle threshold as a turning point; S235, obtaining the turning point closest to the current moment, and obtaining all first bird position points from the turning point to the current moment, and marking them as second bird position points; S236, calculating the second vector from the second bird position point at any moment to the second bird position point at the next moment in chronological order; the calculation formula of the second vector is: In the above formula, The second vector representing the distance from the second bird position at any moment to the second bird position at the next moment, x i+1 ,y i+1 and z i+1 are the X-axis, Y-axis, and Z-axis coordinates of the second bird's position at the next moment, respectively. i ,y i and z i are the X-axis, Y-axis and Z-axis coordinates of the second bird position point at the i-th moment; S237, calculating the sum of the second vectors to obtain a third vector, and establishing a first predicted trajectory extending from the first bird position point at the current moment along the direction of the third vector; the calculation formula of the third vector is: In the above formula, represents the third vector, represents the second vector, and n represents the number of the second vector.

3. The planning method according to claim 1, characterized in that: In step S26, the following steps are specifically included: S261, according to the first predicted trajectory, the second predicted trajectory and the speed of the drone, calculating the first moment when the drone reaches the position on the second predicted trajectory that satisfies the first instruction data; the first moment satisfies the following relationship: In the above formula, x 0 ,y 0 and z 0 They represent the X-axis, Y-axis, and Z-axis coordinates of the drone at the current moment, k represents the kth moment from the current moment, Δt is the time interval between two adjacent moments, and V0 represents the flight speed set by the drone; S262, obtaining the first designated position coordinates of the UAV at the first moment according to the second predicted trajectory; S263: Connect the coordinates of the first designated position and the coordinates of the drone at the current moment to obtain a straight line trajectory.

4. The planning method according to claim 3, characterized in that: In step S28, the following steps are specifically included: S281, establishing an avoidance cylinder with the first predicted trajectory as the central axis, wherein the radius of the avoidance cylinder is equal to the avoidance radius of the first obstacle circle; S282, establishing a first dividing plane parallel to the first predicted trajectory on the straight trajectory, dividing the avoidance cylinder into two halves through the first dividing plane, and extracting the avoidance cylinder with a smaller arc length of the cross section and marking it as a projection cylinder; S283, establishing a second dividing plane perpendicular to the first dividing plane on the straight line trajectory, and obtaining a dividing line of the second dividing plane on the projected cylindrical surface; S284, obtaining the length of the curve and the length of the straight line in the dividing line, and calculating the difference between the two to obtain a first distance difference; the calculation formula of the first distance difference is: ΔS 0 =S C -S L In the above formula, ΔS 0 Indicates the first distance difference between the length of the curve and the length of the straight line in the dividing line, S C Indicates the length of the curve, S L Indicates the length of a straight line; S285, calculating a second time when the drone reaches a position on the second predicted trajectory that satisfies the first instruction data according to the first distance difference, the first predicted trajectory, the second predicted trajectory, and the speed of the drone; S286. Obtain the coordinates of the designated position of the drone at the second moment according to the second predicted trajectory, and establish the shortest path through the projection cylindrical surface according to the coordinates and the coordinates of the current position of the drone to generate a curved trajectory.

5. The planning method according to claim 4, characterized in that: In step S286, the following steps are specifically included: S2861, obtaining the second designated position coordinates of the UAV at the second moment according to the second predicted trajectory; S2862, connecting the coordinates of the second designated position and the coordinates of the drone at the current moment to obtain a straight line trajectory; S2863, obtaining the curve portion corresponding to the dividing line on the projection cylindrical surface according to the straight line trajectory; S2864, establishing a first tangent line between the second designated position coordinates and the dividing line curve portion, and obtaining a first tangent point; S2865, establishing a second tangent line between the current position of the drone and the curve portion of the dividing line, and obtaining a second tangent point; S2866, connect the arc line of the first tangent point and the second tangent point on the projected cylindrical surface, the first tangent line and the second tangent line to establish a curve trajectory.

6. The planning method according to claim 2, characterized in that: In step S233, the calculation formula of the deflection angle is: In the above formula, θ i is the deflection angle of the first bird position point at the i-th moment, Represents the first vector from the first bird position point at the i-th moment to the first bird position point at the next adjacent moment, Represents the first vector from the first bird position point at the i-th moment to the first bird position point at the previous adjacent moment.

7. The planning method according to claim 4, characterized in that: In step S285, the second moment satisfies the following relationship: In the above formula, It represents the first distance difference corresponding to the time when the position in the first instruction data is reached at the i-th moment.

8. A planning system for implementing the UAV trajectory planning method according to any one of claims 1 to 7, characterized in that: It includes a processor and a memory, the memory is used to store a computer program, and when the computer program is executed by the processor, the unmanned aerial vehicle trajectory planning method as described in any one of claims 1 to 7 is implemented.

Images