Trajectory generation method for unmanned aerial vehicle target tracking

Abstract

The application relates to a trajectory generation method for unmanned aerial vehicle target tracking, which comprises an unmanned aerial vehicle with a built-in GPS module and a device with a built-in GPS module worn by a human body; when the human body is in a motion state, the device worn by the human body and the unmanned aerial vehicle generate pseudo codes; pseudo distance measurement is performed at a certain moment of GPS time; a nonlinear equation set related to the pseudo distance is obtained, and repeated iteration calculation is performed to obtain accurate coordinates; the Dijkstra algorithm is used to calculate the shortest path in a weighted directed graph, and the unmanned aerial vehicle flies along the shortest moving trajectory from the starting point to the human body target point; the unmanned aerial vehicle and the device perform pseudo distance measurement at a certain moment of GPS time, repeated iteration calculation is performed to reduce errors, and the accurate coordinates of the device and the unmanned aerial vehicle are obtained; based on the search of the weighted graph, the coordinates of the unmanned aerial vehicle and the human body are regarded as two end points, the starting point and the ending point are represented, and the shortest trajectory in the graph is calculated according to the algorithm.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of unmanned aerial vehicle, in particular to a trajectory generation method for unmanned aerial vehicle target tracking. BACKGROUND

[0002] At present, civil unmanned aerial vehicles are mainly divided into two categories: consumer level and industrial level. Among them, consumer level unmanned aerial vehicles are mainly used in personal aerial photography, entertainment and other fields; industrial level unmanned aerial vehicles have many applications in agriculture, inspection, logistics, rescue and other fields. Due to the advantages of high mobility, low cost and flexible operation, unmanned aerial vehicles have been developed and used unprecedentedly, and tracking targets based on unmanned aerial vehicles is of great value.

[0003] For example, when a consumer level unmanned aerial vehicle is working, it can track and shoot objects from a long distance. It is frequently used by film producers, video bloggers and even ordinary users who want to capture life moments and memories. However, in the current tracking scheme of unmanned aerial vehicles, since the target and the unmanned aerial vehicle are both in motion, the unmanned aerial vehicle will not follow the user when the user moves, and it is particularly important for the unmanned aerial vehicle to generate a tracking trajectory that can quickly reach the target position. SUMMARY

[0004] The present application mainly aims at the problems existing in the above-mentioned unmanned aerial vehicle tracking, and invents a trajectory generation method for unmanned aerial vehicle target tracking. The GPS module is built in the unmanned aerial vehicle and the device worn by the human body. Pseudo-range measurement is performed at a certain time of GPS time, and a nonlinear equation set related to the pseudo-range is calculated and repeatedly iterated to reduce the error caused by atmospheric refraction or the propagation speed of electromagnetic waves in the atmosphere, so as to obtain the coordinates of the device and the unmanned aerial vehicle. In the search of the weighted directed graph, according to whether the edge has a specific direction, the graph can be divided into undirected graph and directed graph. Adding weight to each edge becomes a weighted graph. The coordinates of the unmanned aerial vehicle and the human body are regarded as two end points, representing the starting point and the ending point. The shortest trajectory in the graph is calculated according to the Dijkstra algorithm.

[0005] The purpose of the present application is achieved by the following technical scheme: a trajectory generation method for unmanned aerial vehicle target tracking, using an unmanned aerial vehicle with a built-in GPS module and a device worn by a human body with a built-in GPS module, comprising the following steps:

[0006] S1: setting the initial value me of the distance between the device and the unmanned aerial vehicle and setting the flight trajectory of the unmanned aerial vehicle when the device is stationary;

[0007] S2: when the human body is in a non-stationary state, the device worn by the human body and the unmanned aerial vehicle generate pseudo-codes;

[0008] Perform pseudorange measurement at a certain GPS time; obtain a set of nonlinear equations related to the pseudorange, and repeat the iterative calculation.

[0009] S3: After repeated iterative calculations, the spatial coordinates of the device and the coordinates of the UAV are obtained;

[0010] S4: Calculate the distance between the device and the drone according to the formula of the spatial coordinate system;

[0011] S5: If the distance is less than or equal to the initial distance value me, the drone will fly along the trajectory set by the user in advance;

[0012] S6: When the distance is greater than the initial distance value me, start calculating the drone's trajectory to follow the user's movement;

[0013] The method of calculating the shortest path in a weighted directed graph using Dijkstra's algorithm starts from the starting point and finds the nearest vertex to itself and adds it to the set. After adding it, it finds the nearest vertex to itself again, and continues to find the next nearest vertex to itself and add it to the set each time, until the set destination vertex is added to the set.

[0014] S7: Obtain the shortest movement trajectory from the drone's starting point to the human target point.

[0015] Preferably, the process of obtaining the nonlinear equations related to the pseudorange and repeatedly iterating to obtain the spatial coordinates of the device and the coordinates of the UAV specifically involves:

[0016] At GPS time t, device time t u The advance measure relative to GPS time is denoted as δt. u (t), the relationship between the three is:

[0017] t u (t)=t+δt u (t)

[0018] The satellite time and GPS time of each satellite are not strictly synchronized. At GPS time t, the time of satellite numbered i is denoted as ti. s(i) (t), and its relationship with GPS time is as follows:

[0019] t s(i) (t)=t+δt s(i) (t)

[0020] δt u (t) and δt s(i) (t) are referred to as the device clock bias and the satellite clock bias, respectively. The satellite clock bias can be corrected. The propagation time of the satellite signal electromagnetic wave in the vacuum is τ. Then, the GPS signal sampled by the device at time tu(t) is the GPS satellite signal at time t.s(i) (t-τ) emitted signal. By multiplying the light speed factor c, using the above two equations can be obtained, at t time satellite and device pseudo-range ρ(t) is:

[0021] ρ(t) = c(t u (t)-t s(i) (t-τ)) = cτ + c(δt u (t)-δt s(i) (t-τ)

[0022] Assume in the spatial rectangular coordinate system, the device coordinate vector is [x, y, z] T Vector, the coordinate vector of the nth satellite [x(n), y(n), z(n)], the relevant equation of the device to the satellite distance r is obtained:

[0023]

[0024] Clock difference δtu does not meet the requirements, since the equation set is a nonlinear equation set, it is reduced to a linear equation set by least square method, in K times of iteration, the equation set can be linearized at (x k-1 , y k-1 , z k-1， δt u , k-1), and the derivative of both sides of the equation in the equation set is obtained to obtain a linear equation set:

[0025]

[0026] The left coefficient matrix is marked as U, and the vector on the right side of the equal sign is b, and the least square solution of the equation is:

[0027]

[0028] Finally, the least square solution is obtained at k times of iteration (x k , y k , z k , δt u，k ) to meet the accuracy of Newton iteration convergence or the threshold of clock difference reaches the expectation, the clock accuracy of the device absolute time error is not more than 10ms, otherwise repeat the iteration calculation, and the accurate coordinates of the device and the unmanned aerial vehicle are obtained after the error is met.

[0029] As preferred, the formula of the spatial coordinate system calculates the distance between the device and the unmanned aerial vehicle, and the specific formula is:

[0030]

[0031] Finally, the distance between the device and the unmanned aerial vehicle is obtained.

[0032] As preferred, the method for calculating the shortest path in the weighted directed graph by Dijkstra algorithm is specifically: given the coordinates of the UAV and the device, which can be regarded as two end points, representing the start point and the end point respectively, there are many schemes for the UAV to reach the device, and the shortest path between the two end points can be calculated according to Dijkstra;

[0033] The first time, V2 is selected as the first starting point, and points V3 and V6 adjacent to V2 are unlocked, the distance of the UAV-V2 is updated to 3, the distance of the UAV to V3 is checked, which is UAV-V3=10 or UAV-V2-V3=8, the distance of the UAV to V3 is updated to 8, and the distance of the UAV to V6 is checked, which is only UAV-V2-V6=15, and there is no other path, so the distance of the UAV to V6 is updated to 15;

[0034] The second time, V2 is passed, and V1 is found to be the minimum from the starting point, V3 and V4 adjacent to V1 are unlocked, the distance of the UAV to V3 is checked, which is UAV-V2-V3=8 or UAV-V1-V3=7, the distance of the UAV to V3 is updated to 7, and the distance of the UAV to V4 is checked, which is only UAV-V1-V4=12, and there is no other path, so the distance of the UAV to V4 is updated to 12;

[0035] The third time, V1 and V2 are passed, and V3 is found to be the minimum from the starting point, V5 and V4 adjacent to V3 are unlocked, the distance of the UAV to V4 is checked, which is UAV-V1-V3-V4=10 or UAV-V1-V4=12, and the distance of the UAV to V4 is updated to 10;

[0036] The fourth time, V1, V2 and V3 adjacent to each other are passed, V3 is found to be the closest to V4, V4 and V5 adjacent to V4 are unlocked, the distance of the UAV to V5 is checked, which is UAV-V1-V3-V4-V5=13 or UAV-V1-V3-V5=19, and the distance of the UAV to V5 is updated to 13, and the distance of the UAV to the device is checked, which is UAV-V1-V3-V4-device=15, and there is no other point adjacent to the device at this time, so the distance of the UAV to the device is updated to 15;

[0037] The fifth time, V1, V2, V3 and V4 adjacent to each other are passed, V3 and V5 are found to be the closest, V5 and the device adjacent to V5 are unlocked, the distance of the UAV to the device is checked, which is UAV-V1-V3-V4-device=15 or UAV-V1-V3-V5-device=14, and the distance of the UAV to the device is updated to 14;

[0038] The sixth time, V1, V2, V3, V4 and V5 adjacent to each other are passed, and the device is reached, and it is found that there is no point adjacent to the device, and the process is ended.

[0039] Compared with the prior art, the present application has the following beneficial effects: 1. The unmanned aerial vehicle and the device worn by the human body are both internally provided with a GPS module, pseudo-range measurement is performed at a certain moment in GPS time, a nonlinear equation set related to the pseudo-range is calculated and repeated iteration is performed to reduce errors caused by atmospheric refraction or electromagnetic wave propagation speed in the atmosphere, which is beneficial to obtaining accurate coordinates of the device and the unmanned aerial vehicle; 2. Search based on a weighted directed graph, which can be divided into an undirected graph and a directed graph according to whether the edge has a specific direction, and the weight value is added to each edge to become a weighted graph, the coordinates of the unmanned aerial vehicle and the human body are regarded as two end points, and the start point and the end point are calculated according to the Dijkstra algorithm, which is beneficial to calculating the shortest trajectory in the weighted directed graph. BRIEF DESCRIPTION OF DRAWINGS

[0040] Figure 1 Flowchart provided by the present application;

[0041] Figure 2 Weighted directed graph of the present application;

[0042] Figure 3 Weighted directed graph during trajectory generation operation of the present application;

[0043] Figure 4 Schematic diagram of setting part parameters of the present application. DETAILED DESCRIPTION

[0044] The present application will be further described below in combination with the embodiments shown in the drawings:

[0045] The present application provides a trajectory generation method for unmanned aerial vehicle target tracking, wherein the unmanned aerial vehicle and the device are both internally provided with a GPS module, the device refers to a mobile phone or a watch or other devices internally provided with a GPS, and the human body needs to wear such a device; the GPS system in the unmanned aerial vehicle or the device mainly consists of three parts: a space satellite part, a ground control part and a user receiving part, when the ground control part detects the signal sent by the space satellite, the signal is processed, the running track of the satellite is confirmed, and the information is returned to the satellite, and the satellite rebroadcasts to its signal frequency band after receiving the information; and the unmanned aerial vehicle or the device captures the signal rebroadcasted by the satellite to obtain the spatial coordinates required by itself.

[0046] The space is composed of 24 satellites, and the running period of the satellite on the orbit is 11 hours and 58 minutes, at this time, the earth is also rotating, so the distribution of the satellite will be repeated once for the ground every 23 hours and 56 minutes. This way can make at least 4 satellites be observed at every moment on the earth, and thus more accurate positioning is provided.

[0047] The ground control system comprises a master control station, injection stations, and monitoring stations. The master control station receives and processes data sent by the monitoring stations and issues commands to the injection stations. The master control station acts as the CPU of the entire system, while the injection stations function as command and control centers, transmitting control commands and navigation messages to the satellites under the control of the master control station. This information transmission mechanism—navigation satellite → monitoring station → master control station → injection station → navigation satellite—ensures the normal operation of GPS navigation satellites.

[0048] The trajectory generation method for UAV target tracking provided in this application is described in detail below with reference to the accompanying drawings:

[0049] S1: As Figure 4 As shown, the user pre-sets the initial distance me between the device and the drone and sets the flight path that the drone will capture when the human body is stationary.

[0050] S2: As Figure 1 As shown, when the human body is in a non-stationary state, the device and drone generate pseudo-code to obtain a set of nonlinear equations related to pseudorange. Solving these equations yields the expected values, allowing the device and drone to determine their current positions. The positioning methods for both the device and drone are similar; for example, during optimal positioning of the device, pseudorange needs to be obtained, specifically as follows:

[0051] S21: Perform pseudorange measurement at GPS time t and device time t. u The advance measure relative to GPS time is denoted as δt. u (t), the relationship between the three is:

[0052] t u (t)=t+δt u (t)

[0053] The satellite time and GPS time of each satellite are not strictly synchronized. At GPS time t, the time of satellite numbered i is denoted as ti. s(i) (t), and its relationship with GPS time is as follows:

[0054] t s(i) (t)=t+δt s(i) (t)

[0055] δt u (t) and δt s(i) (t) are referred to as the device clock bias and the satellite clock bias, respectively. The satellite clock bias can be corrected. The propagation time of the satellite signal electromagnetic wave in the vacuum is τ. Then, the GPS signal sampled by the device at time tu(t) is the GPS satellite signal at time t. s(i) The signal emitted is (t-τ). By multiplying by the speed of light factor c and then using the above two equations, we can obtain the pseudorange ρ(t) of the satellite and equipment at time t as follows:

[0056] p(t) = c(t u (t) - t s(i) (t - τ)) = cτ + c(δt u (t) - δt s(i) (t - τ)).

[0057] The pseudo-range is measured by the time of the satellite signal transmission to the device in step S21, however, the clock difference between the two clocks is inevitable, and the signal is also affected by atmospheric refraction and other factors during propagation, and the electromagnetic wave propagates slower in the atmosphere than in the vacuum. The distance measured by the speed of light and time will have a certain error from the directly measured distance, so it is called pseudo-range.

[0058] S22: Assuming that the coordinate vector of the device is [x, y, z] in the space rectangular coordinate system T vector, the coordinate vector of the nth satellite [x(n), y(n), z(n)], and the related equation of the distance r of the device to the satellite is obtained

[0059]

[0060] and the nonlinear equation set

[0061]

[0062] S23: The clock difference δtu does not meet the requirements. Since the equation set is a nonlinear equation set, it is reduced to a linear equation set by the least square method. In K times of iteration, the equation set can be linearized at (x k-1 , y k-1 , z k-1 , δt u , k-1). The derivative of both sides of the equation in the above equation set is obtained to obtain a linear equation set

[0063]

[0064] Then, the coefficient matrix on the left side is marked as U, and the vector on the right side of the equal sign is b. Then, the least square solution of this equation is

[0065]

[0066] The least square solution is obtained at the kth iteration (x k , y k , z k , δt u，k ) to meet the accuracy of Newton iteration convergence or the threshold of the clock difference to reach the expectation, that is, the absolute time error of the clock accuracy of the device does not exceed 10 ms. Otherwise, the iteration calculation is repeated.

[0067] The pseudo code in step S2 is noise code PRN, which has good correlation close to random sequence and predetermined sequence that can be reused, and is very suitable for use in communication system for sending and receiving GPS signals.

[0068] S3: The clock difference δtu meets the requirements, stop calculating, and get the spatial coordinates of the device and the device coordinates of the UAV.

[0069] S4: Use the formula of the spatial coordinate system

[0070] Calculate the distance di between the device and the UAV.

[0071] S5: The distance di is less than or equal to the set value, and the UAV will fly according to the trajectory set by the user in advance.

[0072] S6: di is greater than the set value me, and the UAV calculates the moving trajectory to reach the human body, specifically:

[0073] S61: Plan the optimal path in the known environment, and the UAV is built-in with a navigation system, the key technologies of navigation include positioning and path planning, the positioning has been calculated in step S4, mainly based on search with weighted directed graph.

[0074] S62: Please refer to the attached Figure 3 , according to the coordinates of the UAV and the device calculated in step S4, and as two endpoints, respectively representing the starting point and the ending point, there are many options for the UAV to reach the device, according to Dijkstra algorithm, the shortest path between the two endpoints can be calculated, specifically:

[0075] S621: First select V2 as the first starting point, and unlock the points V3, V6 adjacent to V2, the distance of the UAV to V2 is updated to 3, check the distance of the UAV to V3, which is UAV-V2-V3=10 or UAV-V2-V3=8, update the distance of the UAV to V3 to 8, check the distance of the UAV to V6, which is only UAV-V2-V6=15, there is no other path, and the distance of the UAV to V6 is updated to 15.

[0076] S622: The second V2 has passed, check the minimum V1 from the starting point, unlock V3V4 adjacent to V1, check the distance of the UAV to V3, which is UAV-V2-V3=8 or UAV-V1-V3=7, update the distance of the UAV to V3 to 7, check the distance of the UAV to V4, which is UAV-V1-V4=12, there is no other path, and the distance of the UAV to V4 is updated to 12.

[0077] S623: The third time V1V2 walks through, finds the starting point V3 minimum, and the V3-adjacent V5V4 is unlocked. The distance from the UAV to V4 is updated to 10.

[0078] S624: The fourth time V1V2V3 adjacent to each other, V3 and V4 are the closest, and the V4-adjacent device and V5 are unlocked. The distance from the UAV to V5 is updated to 13, and the distance from the UAV to the device is updated to 15. At this time, there is no other vertex adjacent to the device, and the distance from the UAV to the device is updated to 15.

[0079] S625: The fifth time V1V2V3V4 adjacent to each other, V3 and V5 are the closest, and the V5-adjacent device is unlocked. The distance from the UAV to the device is updated to 14.

[0080] S626: The sixth time V1V2V3V4V5 adjacent to each other, the device is reached, and no point adjacent to the device is found. The UAV and the device move to obtain new spatial coordinates

[0081] Please refer to the description attached Figure 2 In step S62, the Dijkstra algorithm: based on the set of vertices and edges between vertices, it can be divided into undirected graph and directed graph according to whether the edge has a specific direction, and the weight is added to each edge to become a weighted graph. The set initially contains only the starting point. From the starting point x0, the nearest vertex is found and added to the set. After adding, the nearest vertex is found again, and each time the nearest vertex is found on the basis of the last time and added to the set. Until the set S contains the set of vertices, this path is the shortest path from the starting point to the destination point.

[0082] S7: The UAV moves according to the optimal path calculated in steps S621 to S626.

[0083] In step S7, the UAV and the device move to obtain new spatial coordinates. Through Dijkstra and weighted directed graph, the shortest path can be planned to enable the UAV to track the device along the optimal path to achieve navigation.

[0084] The specific embodiments described herein are merely illustrative of the spirit of the application. Various modifications or changes in addition or substitution to the described specific embodiments can be made by those skilled in the art without departing from the spirit of the application or exceeding the scope of the appended claims.

Claims

1.A method for trajectory generation for UAV target tracking, characterized in that, Drones with built-in GPS modules and devices worn by humans with built-in GPS modules include the following steps: S1: Establish the initial distance me between the wearable device and the drone, and establish the flight trajectory captured by the drone when the device is stationary; S2: When the device detects that a human body is not stationary, the device and the drone generate pseudo-code; Perform pseudorange measurement at a certain GPS time; obtain a set of nonlinear equations related to the pseudorange, and repeat the iterative calculation. S3: After repeated iterative calculations by the GPS modules in the device and the drone, the spatial coordinates of the device and the coordinates of the drone are obtained; S4: Formula for establishing a spatial coordinate system for the UAV; calculates the distance di between the device and the UAV. S5: When the distance di is less than or equal to the initial distance me, the drone will fly according to the trajectory set by the user in advance; S6: When the distance di of the drone is greater than the initial distance value me, the drone begins to calculate the movement trajectory of the following device; The calculation adopts Dijkstra's algorithm to calculate the shortest path in a weighted directed graph. It starts from the starting point, finds the vertex closest to itself, and adds it to the set. After adding it, it finds the vertex closest to itself again. Each time, it continues to find the next closest vertex based on the previous one and adds it to the set, until the set destination vertex is added to the set. S7: The drone obtains the shortest trajectory from the starting point to the target point of the device and moves accordingly. 2.The method of claim 1, wherein, The nonlinear equations relating to the pseudorange are obtained, and the spatial coordinates of the device and the UAV are calculated iteratively over time. Specifically: At GPS time t, device time t u The amount of advance with respect to GPS time is denoted by the device clock error and its expression is δt u (t), the relationship between the three is: t u (t) = t + δt u (t) The satellite time of each satellite and the GPS time are also not strictly synchronized. At the GPS time t, the satellite time of the satellite numbered i is recorded as t s(i) (t), which is related to the GPS time as follows: t s(i) (t) = t + δt s(i) (t) δt u (t) and δt s(i) (t) are called as equipment clock error and satellite clock error respectively, the satellite clock error can be corrected, the time of satellite signal electromagnetic wave propagating in vacuum is τ, then the equipment clock error is δt u (t) time sampling GPS signal is the signal emitted by GPS satellite at t s(i) (t-τ); through multiplying by light speed factor c, using the above two formulas can get the pseudo range ρ(t) of satellite and equipment at t time: p(t) = c(t u (t) - t s(i) (t - τ)) = cτ+ c(δt u (t) - δt s(i) (t - τ)) Assume that the coordinate vector of the device is [x, y, z] in the space rectangular coordinate system T Vector, the coordinate vector of the nth satellite [x(n), y(n), z(n)], obtains the relevant equation of the distance r of the device to the satellite: Equipment clock difference δt u (t) does not meet the requirements, because the equation set is a nonlinear equation set, it is reduced to a linear equation set by least square method, at K times of iteration, the equation set can be linearized at (x k-1 , y k-1 , z k-1 , δt u , k-1 ) and the linear equation set is obtained by derivation on both sides of the equation of the distance r of the above equipment to the satellite. Let U be the coefficient matrix on the left and b be the vector on the right. Then the least squares solution to this equation is: Finally in k times of iteration of (x k , y k , z k , δt u，k ) find the least square solution meets the accuracy of Newton iteration convergence or the threshold of equipment clock difference reaches the expectation, the clock accuracy absolute time error is not more than 10 ms, otherwise repeat the iteration calculation, and the accurate coordinates of the equipment and the unmanned aerial vehicle are obtained after the error is met. 3.The method of claim 1, wherein, Calculate the distance between the device and the drone using the formula for the distance between any two points A and B in a spatial coordinate system: Finally, the distance between the device and the drone is denoted as di. 4.The method of claim 2, wherein, The method for calculating the shortest path in a weighted directed graph using Dijkstra's algorithm is as follows: Given the coordinates of the drone and the device, which can be considered as two endpoints representing the start and end points respectively, there are many routes the drone can take to reach the device. The shortest path between the two endpoints can be calculated using Dijkstra's algorithm. The first time, V2 is selected as the first starting point, and the adjacent points V3 and V6 are unlocked. The distance from the drone to V2 is updated to 3. The distance from the drone to V3 is either 10 or 8. The distance from the drone to V3 is updated to 8. The only path from the drone to V6 is 15. There are no other paths. The distance from the drone to V6 is updated to 15. The second time V2 walks through, check the minimum V1 from the starting point, unlock V3, V4 adjacent to V1, check the distance from the UAV to V3: UAV-V2-V3 distance is 8 or UAV-V2-V3 distance is 8 or UAV-V1-V3 distance is 7, the distance from the UAV to V3 is updated to 7, check the distance from the UAV to V4: UAV-V1-V4 distance is 12, there is no other path, the distance from the UAV to V4 is updated to 12; The third time V1, V2 walks through, finds that V3 from the starting point is the smallest, unlocks V5, V4 near V3, checks the distance from the UAV to V4: UAV-V1-V3-V4 distance is 10 or UAV-V1-V4 distance is 12, the distance from the UAV to V4 is updated to 10; The fourth time V1, V2, V3 walks through, V3 is closest to V4, unlock devices adjacent to V4 and V5, check the distance from the UAV to V5: UAV-V1-V3-V4-V5 distance is 13 or UAV-V1-V3-V5 distance is 19, the distance from the UAV to V5 is updated to 13, check the distance from the UAV to the device: UAV-V1-V3-V4-device distance is 15, at this time there is no other vertex and device adjacent, the distance from the UAV to the device is updated to 15; The fifth time V1, V2, V3, V4 walks through, V3 and V5 are the closest, unlock devices adjacent to V5, check the distance from the UAV to the device: UAV-V1-V3-V4-device distance is 15 or UAV-V1-V3-V5-device distance is 14, the distance from the UAV to the device is updated to 14; The sixth time V1, V2, V3, V4, V5 walks through, reaches the device, checks that there is no point adjacent to the device, ends.

Images