A vision-based drone tracking drone method

Abstract

The application belongs to the technical field of unmanned aerial vehicle, and discloses a method for tracking unmanned aerial vehicle based on vision, which comprises the following steps: step 1, detecting target unmanned aerial vehicle and estimating its position; step 2, predicting the motion trajectory of the target unmanned aerial vehicle; step 3, front-end path search; and step 4, back-end trajectory optimization. The method used in the application has high flexibility, the tracking unmanned aerial vehicle can make a timely response according to the motion state of the target unmanned aerial vehicle, so as to ensure that the target unmanned aerial vehicle does not deviate from the field of view of the tracking unmanned aerial vehicle; the safety is high, the trajectory of the tracking unmanned aerial vehicle is limited in a safe area as close to the target unmanned aerial vehicle as possible, so that the two can maintain a relatively safe distance. The application uses a lightweight target detection model and efficient path search and trajectory optimization algorithms, has fast calculation speed, has low hardware requirements for the tracking unmanned aerial vehicle, and only needs one on-board computer to meet all calculations, thereby greatly reducing the weight of the tracking unmanned aerial vehicle.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned aerial vehicles (UAVs), and in particular relates to a vision-based UAV tracking UAV method. Background Art

[0002] In recent years, drones have experienced rapid development in military, industrial, and civilian applications. Drone tracking, a key branch of drone technology, has been widely used in inspection, rescue, and surveillance. Drone tracking involves multiple technologies, including image processing, trajectory prediction, path search, trajectory optimization, and motion control. Different technical approaches are required for different tracking targets and application scenarios. Tracking drones places higher demands on algorithms than tracking ground objects. As aerial targets, drones have one more degree of freedom than ground objects and are more flexible, making their trajectory more difficult to predict. Furthermore, the small size of most drones places higher demands on the accuracy of target detection models. At the same time, the detection models must be lightweight, as the computing power of drones' onboard computers is very limited.

[0003] Drone tracking has broad application prospects in areas such as drone navigation and drone interception. However, existing drone tracking algorithms mainly focus on tracking ground targets, and there is still a large gap in tracking algorithms that can track drones as targets. Summary of the Invention

[0004] The purpose of the present invention is to provide a vision-based drone tracking method to solve the above-mentioned technical problems.

[0005] To solve the above technical problems, the specific technical solution of the vision-based UAV tracking method of the present invention is as follows:

[0006] A vision-based UAV tracking method includes the following steps:

[0007] Step 1: Detect the target drone and estimate its position: A monocular camera is mounted on the tracker drone and the YOLOv5 object detection algorithm is used to detect the target drone.

[0008] Step 2: Predict the trajectory of the target drone; assume that the current position of the target drone is z0, the current velocity and acceleration are v0 and a0 respectively, and the time T p After that, its position is z p , using the Hybrid A* algorithm, search for a path from z0 to

[0009] z p The path is used as the predicted trajectory of the target UAV;

[0010] Step 3: Front-end path search: Using the JPS algorithm, search for N JPS paths from the current position s0 of the tracking drone through Φ0, Φ1, ..., Φ N , the starting point s of the nth JPS path n The end point e of the previous JPS path n-1 ;

[0011] Step 4: Back-end trajectory optimization: The JPS trajectory generated in step 3 is optimized using the MINCO algorithm to obtain a smooth, safe trajectory that satisfies dynamic constraints.

[0012] Furthermore, step 1 uses the deep learning inference optimizer - TensorRT to speed up the detection.

[0013] Furthermore, the step 1 includes the following specific steps:

[0014] Consider the rotorcraft as a cylinder, that is, its width is the same in all directions when viewed from the horizontal plane.

[0015] The width of the target detection frame is w0, and the center point coordinates are [x0, y0]. The target detection frame information and the camera internal parameters are used to estimate the target drone position:

[0016]

[0017]

[0018]

[0019] Among them, z c (0), z c (1) z c (2) represent the x-axis, y-axis, and z-axis coordinates of the target drone in the camera coordinate system; w u represents the width of the target UAV; f x 、f y Respectively represent the focal length of the camera on the x-axis and y-axis;

[0020] Finally, a Kalman filter is used to smooth the position of the target drone and estimate its velocity and acceleration. Furthermore, step 2 includes the following specific steps:

[0021] Assume that the current position of the target drone is z0, the current velocity and acceleration are v0 and a0 respectively, and after time T p After that, its position z p Expressed as:

[0022]

[0023] In order to make the tracking UAV quickly respond to the change of the target UAV's z-axis speed, p Add a bias to the z-axis component:

[0024] z p (2) = z p (2)+Δf

[0025] Δf=k1(z0(2)-z tr0 (2))+k2(v0(2)-v tr0 (2)

[0026] Among them, z tr0 、v tr0 Respectively represent the position and speed of the tracking drone; k1 and k2 are adjustable constants. Furthermore, the above step 2 uses the Hybrid A* algorithm to search for a path from z0 to z p The path is used as the predicted trajectory of the target UAV. In order to speed up the efficiency of the Hybrid A* search path, when expanding the node, it is hoped that the acceleration a(n) of each node can be close to the current acceleration a0 of the target UAV. Therefore, the cost function and heuristic function are selected as follows:

[0027] f(n)=g(n)+h(n)

[0028] g(n)=g(n-1)+||a(n)-a0||

[0029] h(n)=0.01·||z(n)-z p ||

[0030] Where g(n) and h(n) represent the cost function and heuristic function of the n-th node respectively; f(n) represents the total cost of the n-th node; z(n) and a(n) represent the position of the n-th node and the acceleration of its extended trajectory respectively.

[0031] Each node z(n) expanded by Hybrid A* corresponds to each position prediction point z of the target drone n If there are N nodes in total, the target UAV’s position prediction point can be expressed as:

[0032]

[0033] Furthermore, in step 3, the horizontal distance between the tracking drone and the target drone is kept at [d l ,d h ], the vertical distance is kept at [-h,h], for each predicted point z of the target UAV n Set a three-dimensional circular safety area Φ n The inner and outer diameters of the ring are dl and d h , the height of the ring is 2h. When the tracking UAV leaves the safety zone Φ0 of the target UAV’s current position z0, the front-end path search will begin.

[0034] Furthermore, the front-end path search in step 3 includes the following specific steps:

[0035] Using the JPS algorithm, search for N JPS paths from the current position s0 of the tracking drone through Φ0, Φ1, ..., Φ N , the starting point s of the nth JPS path n The end point e of the previous JPS path n-1 , each end point e n The determination method includes: 1) setting the starting point s of the nth JPS path n Projected to the target UAV’s nth position prediction point z n xy plane Ψ n , denoted as s n ';

[0036] 2) With z n is the center of the circle, (d l +d h ) / 2 is the radius of the circle O n ;

[0037] 3) Connect s′ n 、z n , line segment s′ n z n With O n The intersection point is e n ;

[0038] 4) If e n If there is an obstacle, then n From e n Find the nearest point e without obstacles on both sides n ′ as the end point; the cost function g of the nth JPS path n (k) and the heuristic function h n (k) is defined as:

[0039] f n (k) = g n (k)+h n (k)

[0040] g n (k) = g n (k-1)+||z n (k)-s n ||

[0041] h n(k)=||z n (k)-e n ||

[0042] where z n (k) represents the position of the kth node.

[0043] Furthermore, the step 4 includes the following specific steps:

[0044] MINCO is an optimization-based trajectory planning framework for multirotor aircraft. It enables the trajectory to achieve minimum control force while satisfying geometric configuration constraints and user-defined dynamic constraints. The trajectory in MINCO is represented as:

[0045]

[0046] Where p(t) is an m-dimensional polynomial trajectory, which is divided into M segments, and each segment is expressed as:

[0047]

[0048] where c i represents the polynomial coefficient, β(t) represents the time T i The column vector composed of each order term, q and T represent the intermediate path points and the duration of each trajectory respectively. In MINCO, each trajectory segment is represented by only two variables, q and T.

[0049] Furthermore, the optimization of the trajectory in step 4 is achieved by solving the following quadratic convex optimization problem:

[0050]

[0051]

[0052]

[0053]

[0054]

[0055]

[0056]

[0057] The first term is the target loss function to be optimized, which is expressed as a trade-off between acceleration loss and time loss, and ρ is a constant; the second term represents the position constraints of the starting and ending points of the trajectory; the third term represents the constraints of maximum speed and acceleration; the fourth term is the flight corridor constraint, which ensures that the trajectory remains in an obstacle-free area. The fifth item is the visible area constraint, each target prediction point zn There is a visible area Ensure that the tracking drone detects the target drone in real time; the sixth and seventh items are the distance constraints between the tracking drone and the target drone, d xy (p(t n ),z n ) and d z (p(t n ),z n ) represent the horizontal and vertical distances between them respectively;

[0058] Solve the optimal trajectory by calculating the gradient:

[0059]

[0060]

[0061] The JPS path is optimized to become a smooth trajectory.

[0062] The vision-based UAV tracking method of the present invention has the following advantages:

[0063] (1) The method used in the present invention is highly flexible. The tracking drone can respond promptly according to the motion state of the target drone, ensuring that the target drone does not leave the field of view of the tracking drone.

[0064] (2) The method used in the present invention is highly safe. The trajectory of the tracking drone is limited to a safe area near the target drone as much as possible, so that the two can maintain a relatively safe distance.

[0065] (3) The method used in the present invention is highly robust and can track the target drone safely and stably for a long time.

[0066] (4) The method used in the present invention is highly efficient, using a lightweight target detection model and an efficient path search and trajectory optimization algorithm. It has a fast calculation speed and low hardware requirements for tracking drones. Only one onboard computer is needed to meet all calculations, which greatly reduces the load on the tracking drone. BRIEF DESCRIPTION OF THE DRAWINGS

[0067] Figure 1 This is a schematic diagram of target detection box information;

[0068] Figure 2 This is a schematic diagram of the target UAV trajectory prediction;

[0069] Figure 3 This is a schematic diagram of the JPS path search;

[0070] Figure 4 Schematic diagram of the method for determining the starting and ending points of each JPS path;

[0071] Figure 5 This is a schematic diagram of the visible range of the target drone;

[0072] Figure 6 Schematic diagram of the flight corridor for tracking drones;

[0073] Figure 7 Optimize the schematic diagram for the trajectory;

[0074] Figure 8 Schematic diagram of specific embodiment 1 of the present invention. DETAILED DESCRIPTION

[0075] In order to better understand the purpose, structure and function of the present invention, the following is a further detailed description of a vision-based drone tracking drone method of the present invention in conjunction with the accompanying drawings.

[0076] The tracking UAV and the target UAV are both quadrotor UAVs, and the horizontal distance between them is kept at [d l ,d h ], and the vertical distance is maintained at [-h,h]. When the target drone starts to move and the tracking drone exceeds the safety range, it will start planning a path to track the target drone. The algorithm process is mainly divided into the following steps:

[0077] (1) Detect the target drone and estimate its position:

[0078] The tracking drone is equipped with a monocular camera and uses the YOLOv5 target detection algorithm to detect the target drone, and uses the deep learning inference optimizer - TensorRT to speed up the detection speed.

[0079] Since the rotor UAV is centrally symmetrical and the roll angle does not change much during movement, it can be regarded as a cylinder, that is, its width is the same when viewed from all directions in the horizontal plane.

[0080] like Figure 1 As shown in the figure, the width of the target detection frame is w0, and the center point coordinates are [x0, y0]. The target detection frame information and the camera intrinsic parameters can be used to estimate the target drone position:

[0081]

[0082]

[0083]

[0084] Among them, z c (0), z c (1) z c (2) represent the x-axis, y-axis, and z-axis coordinates of the target drone in the camera coordinate system; wu represents the width of the target UAV; f x 、f y Represents the focal length of the camera along the x-axis and y-axis respectively.

[0085] Finally, a Kalman filter is used to smooth the target drone’s position and estimate its velocity and acceleration.

[0086] (2) Predicting the trajectory of the target drone:

[0087] like Figure 2 As shown, assuming that the current position of the target drone is z0, the current velocity and acceleration are v0 and a0 respectively, and after time T p After that, its position z p It can be expressed as:

[0088]

[0089] In order to make the tracking UAV quickly respond to the change of the target UAV's z-axis speed, p Add a bias to the z-axis component:

[0090] z p (2) = z p (2)+Δf

[0091] Δf=k1(z0(2)-z tr0 (2))+k2(v0(2)-v tr0 (2)

[0092] Among them, z tr0 、v tr0 They represent the position and speed of the tracking drone respectively; k1 and k2 are adjustable constants.

[0093] Use the Hybrid A* algorithm to search for a path from z0 to z p The path is used as the predicted trajectory of the target drone. To improve the efficiency of the Hybrid A* path search, when expanding the node, we hope that the acceleration a(n) of each node can be close to the current acceleration a0 of the target drone. Therefore, the cost function and heuristic function are selected as follows:

[0094] f(n)=g(n)+h(n)

[0095] g(n)=g(n-1)+||a(n)-a0||

[0096] h(n)=0.01·||z(n)-z p ||

[0097] Where g(n) and h(n) represent the cost function and heuristic function of the n-th node respectively; f(n) represents the total cost of the n-th node; z(n) and a(n) represent the position of the n-th node and the acceleration of its extended trajectory respectively.

[0098] Each node z(n) expanded by Hybrid A* corresponds to each position prediction point z of the target drone n If there are N nodes in total, the target UAV’s position prediction point can be expressed as:

[0099]

[0100] (3) Front-end path search

[0101] like Figure 3 As shown in the figure, in order to keep the horizontal distance between the tracking UAV and the target UAV at [d l ,d h ], the vertical distance is kept at [-h,h], for each predicted point z of the target UAV n Set a three-dimensional circular safety area Φ n The inner and outer diameters of the ring are d l and d h , the height of the ring is 2h. When the tracking UAV leaves the safety zone Φ0 of the target UAV’s current position z0, the front-end path search will begin.

[0102] Using the JPS algorithm, search for N JPS paths from the current position s0 of the tracking drone through Φ0, Φ1, ..., Φ N The starting point s of the nth JPS path n The end point e of the previous JPS path n-1 . Figure 4 Shows each endpoint n Determined method:

[0103] 1) Set the starting point s of the nth JPS path n Projected to the target UAV’s nth position prediction point z n xy plane Ψ n , denoted as s n ';

[0104] 2) With z n is the center of the circle, (d l +d h ) / 2 is the radius of the circle O n ;

[0105] 3) Connect s′ n 、z n , line segment s′ n z n With On The intersection point is e n ;

[0106] 4) If e n If there is an obstacle at O n From e n Find the nearest point e without obstacles on both sides n ’ as the end point.

[0107] The cost function g of the nth JPS path n (k) and the heuristic function h n (k) is defined as:

[0108] f n (k) = g n (k)+h n (k)

[0109] g n (k) = g n (k-1)+||z n (k)-s n ||

[0110] h n (k)=||z n (k)-e n ||

[0111] where z n (k) represents the position of the kth node.

[0112] (4) Back-end trajectory optimization

[0113] The JPS trajectory generated in step (3) is optimized using the MINCO algorithm to obtain a smooth, safe trajectory that satisfies dynamic constraints.

[0114] MINCO is an optimization-based trajectory planning framework for multirotor aircraft that enables trajectories to achieve minimum control forces while satisfying geometric configuration constraints and user-defined dynamic constraints. The trajectory in MINCO is represented as:

[0115]

[0116] Where p(t) is an m-dimensional polynomial trajectory, which is divided into M segments, and each segment is expressed as:

[0117]

[0118] where c i represents the polynomial coefficient, β(t) represents the time T iA column vector of terms. q and T represent the intermediate path points and the duration of each trajectory segment, respectively. In MINCO, each trajectory segment can be represented by only two variables, q and T.

[0119] Furthermore, the trajectory is optimized by solving the following quadratic convex optimization problem:

[0120]

[0121]

[0122]

[0123]

[0124]

[0125]

[0126]

[0127] The first item is the target loss function to be optimized, which is expressed as a trade-off between acceleration loss and time loss, and ρ is an adjustable constant; the second item represents the position constraints of the trajectory start and end points; the third item represents the maximum speed and acceleration constraints; the fourth item is the flight corridor constraint, such as Figure 5 As shown, ensure that the trajectory remains in the obstacle-free area The fifth item is the visible area constraint, such as Figure 6 As shown, each target prediction point z n There is a visible area Ensure that the tracking drone can detect the target drone in real time; the sixth and seventh items are the distance constraints between the tracking drone and the target drone, d xy (p(t n ),z n ) and d z (p(t n ),z n ) represent the horizontal and vertical distances between them respectively.

[0128] Furthermore, the optimal trajectory can be solved by calculating the gradient:

[0129]

[0130]

[0131] like Figure 7 As shown in Figure 3, the JPS path is optimized and eventually becomes a smooth trajectory.

[0132] Example 1:

[0133] like Figure 8 As shown in the figure, Drone B needs to reach Destination G, but it has no map of its surroundings and no information about the destination, making it unable to reach it on its own. Drone A, however, has a map of its surroundings and destination information, and can plan a suitable path to reach it. Therefore, Drone A can be used as the pilot, and Drone B can track Drone A, navigating the obstacle area to reach Destination G.

[0134] It will be understood that the present invention is described by way of some embodiments, and it will be appreciated by those skilled in the art that various changes or equivalent substitutions may be made to these features and embodiments without departing from the spirit and scope of the present invention. In addition, under the teachings of the present invention, these features and embodiments may be modified to adapt to specific circumstances and materials without departing from the spirit and scope of the present invention. Therefore, the present invention is not limited to the specific embodiments disclosed herein, and all embodiments falling within the scope of the claims of this application are intended to be protected by the present invention.

Claims

1. A vision-based drone tracking method, characterized in that: The steps include: Step 1: Detect the target drone and estimate its position: A monocular camera is mounted on the tracker drone and the YOLOv5 object detection algorithm is used to detect the target drone. Step 2: Predict the trajectory of the target drone; assume that the current position of the target drone is z0, the current velocity and acceleration are v0 and a0 respectively, and the time T p After that, its position is z p , using the Hybrid A* algorithm, search for a path from z0 to z p The path is used as the predicted trajectory of the target UAV; Step 3: Front-end path search: Using the JPS algorithm, search for N JPS paths from the current position s0 of the tracking drone through Φ0, Φ1, ..., Φ N , the starting point s of the nth JPS path n The end point e of the previous JPS path n-1 ; Using the JPS algorithm, search for N JPS paths from the current position s0 of the tracking drone through Φ0, Φ1, ..., Φ N , the starting point s of the nth JPS path n The end point e of the previous JPS path n-1 , each end point e n Methods for determination include: 1) Set the starting point s of the nth JPS path n Projected to the target UAV’s nth position prediction point z n xy plane Ψ n , denoted as s n '; 2) With z n is the center of the circle, (d l +d h ) / 2 is the radius of the circle O n ; 3) Connect s′ n 、z n , line segment s′ n z n With O n The intersection point is e n ; 4) If e n If there is an obstacle, then n From e n Find the nearest point e without obstacles on both sides n ' as the end point; The cost function g of the nth JPS path n (k) and the heuristic function h n (k) is defined as: f n (k)=g n (k)+h n (k) g n (k)=g n (k-1)+‖z n (k)-s n ‖ h n (k)=‖z n (k)-e n ‖ where z n (k) represents the position of the kth node; Step 4: Back-end trajectory optimization: The JPS trajectory generated in step 3 is optimized using the MINCO algorithm to obtain a smooth, safe trajectory that satisfies dynamic constraints.

2. The method for tracking drones according to claim 1, wherein: Step 1 uses the deep learning inference optimizer, TensorRT, to speed up detection.

3. The method for tracking drones according to claim 1, wherein: The step 1 includes the following specific steps: Consider the rotorcraft as a cylinder, that is, its width is the same in all directions when viewed from the horizontal plane. The width of the target detection frame is w0, and the center point coordinates are [x0, y0]. The target detection frame information and the camera internal parameters are used to estimate the target drone position: Among them, z c (0), z c (1) z c (2) represent the x-axis, y-axis, and z-axis coordinates of the target drone in the camera coordinate system; w u represents the width of the target UAV; f x 、f y Respectively represent the focal length of the camera on the x-axis and y-axis; Finally, a Kalman filter is used to smooth the target drone’s position and estimate its velocity and acceleration.

4. The method for tracking drones according to claim 1, wherein: The step 2 includes the following specific steps: Assume that the current position of the target drone is z0, the current velocity and acceleration are v0 and a0 respectively, and after time T p After that, its position z p Expressed as: In order to make the tracking UAV quickly respond to the change of the target UAV's z-axis speed, p Add a bias to the z-axis component: z p (2)=z p (2)+Δf Δf=k1(z0(2)-z tr0 (2))+k2(v0(2)-v tr0 (2)) Among them, z tr0 、v tr0 They represent the position and speed of the tracking drone respectively; k1 and k2 are adjustable constants.

5. The method for tracking drones according to claim 1, wherein: Step 2 uses the HybridA* algorithm to search for a path from z0 to z p The path is used as the predicted trajectory of the target UAV. In order to speed up the efficiency of the HybridA* path search, when expanding the node, it is hoped that the acceleration a(n) of each node can be close to the current acceleration a0 of the target UAV. Therefore, the cost function and heuristic function are selected as follows: f(n)=g(n)+h(n) g(n)=g(n-1)+‖a(n)-a0‖ h(n)=0.01·‖z(n)-z p ‖ Where g(n) and h(n) represent the cost function and heuristic function of the nth node respectively; f(n) represents the total cost of the nth node; z(n) and a(n) represent the position of the nth node and the acceleration of its extended trajectory respectively. Each node z(n) expanded by Hybrid A* corresponds to each position prediction point z of the target drone n， If there are N nodes in total, the target UAV’s position prediction point can be expressed as:

6. The method for tracking drones according to claim 1, wherein: In step 3, the horizontal distance between the tracking UAV and the target UAV is kept at [d l ,d h ], the vertical distance is kept at [-h,h], for each predicted point z of the target UAV n Set a three-dimensional circular safety area Φ n The inner and outer diameters of the ring are d l and d n , the height of the ring is 2h. When the tracking UAV leaves the safety zone Φ0 of the target UAV’s current position z0, the front-end path search will begin.

7. The method for tracking drones according to claim 1, wherein: The step 4 includes the following specific steps: MINCO is an optimization-based trajectory planning framework for multirotor aircraft. It enables the trajectory to achieve minimum control force while satisfying geometric configuration constraints and user-defined dynamic constraints. The trajectory in MINCO is represented as: Where p(t) is an m-dimensional polynomial trajectory, which is divided into M segments, and each segment is expressed as: where c i represents the polynomial coefficient, β(t) represents the time T i The column vector composed of each order term, q and T represent the intermediate path points and the duration of each trajectory respectively. In MINCO, each trajectory segment is represented by only two variables, q and T.

8. The method for tracking drones according to claim 7, wherein: The optimization of the trajectory of step 4 is achieved by solving the following quadratic convex optimization problem: The first term is the target loss function to be optimized, which is expressed as a trade-off between acceleration loss and time loss, and ρ is a constant; the second term represents the position constraints of the starting and ending points of the trajectory; the third term represents the constraints of maximum speed and acceleration; the fourth term is the flight corridor constraint, which ensures that the trajectory remains in an obstacle-free area. The fifth item is the visible area constraint, each target prediction point z n There is a visible area Ensure that the tracking drone detects the target drone in real time; the sixth and seventh items are the distance constraints between the tracking drone and the target drone, d xy (p(t n ),z n ) and d z (p(t n ),z n ) represent the horizontal and vertical distances between them respectively; Solve the optimal trajectory by calculating the gradient: The JPS path is optimized to become a smooth trajectory.

Images