An unmanned aerial vehicle all-onboard dynamic target tracking and grabbing method

Abstract

The application discloses a kind of unmanned vehicle full airborne dynamic target tracking and grabbing method, can be realized in complex environment to the air tracking and grabbing of mobile target.The existing research has great limitation in processing mobile target, avoiding collision and realizing full autonomous operation on completely airborne platform.In order to make up for this defect, the application proposes a full airborne scheme integrated with robust visual perception and two-stage (tracking and grabbing) hybrid trajectory planner.Through high-frequency target detection, self-positioning and mapping, obstacles can be effectively avoided, target visibility is maintained, and finally precise grabbing in complex environment is realized.In addition, when facing asymmetric objects, the grabbing direction can be adjusted to grab from the most suitable direction.Simulation and real experiment results show that the method proposed in the application performs superior performance and robustness in complex dynamic grabbing tasks, thereby providing important technical support for highly autonomous aerial operations.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) control technology, specifically to a method for tracking and capturing dynamic targets on a UAV with full airborne capability. Background Technology

[0002] In scenarios such as logistics delivery, emergency rescue, and industrial maintenance, drones often need to follow, approach, and perform grabbing or retrieval operations on ground or aerial targets. These tasks are often conducted in complex environments where the target may move with the carrier and there are obstacles around it. This requires drones to maintain target visibility and approach from the appropriate direction while ensuring safety, thereby improving the success rate of grabbing and operational efficiency.

[0003] In existing technologies, one type of solution focuses on the grasping mechanism itself or simple distance control, lacking real-time mapping of complex obstacle environments and constraints on safe and feasible domains, which easily leads to collision risks; another type of solution can perform target recognition or tracking, but it is insufficient in predicting target motion or fails to incorporate visibility constraints and grasping direction constraints into a unified plan, resulting in the target easily leaving the field of view and poor grasping posture in the near-end stage, thus reducing the success rate; at the same time, solutions that rely heavily on external positioning or ground computing power are difficult to meet the requirements of full airborne real-time performance and general deployment.

[0004] Therefore, there is an urgent need for a UAV dynamic target tracking and grasping method that can achieve stable perception and state estimation of dynamic targets, target motion prediction, phased safety planning and trajectory optimization under full airborne conditions, and can adaptively control the gripper deformation according to distance to complete the grasping, so as to improve the real-time performance, safety and grasping success rate in complex environments.

[0005] A search revealed a Chinese patent document disclosing an autonomous grasping device and its control method for unmanned aerial vehicles (UAVs) [Application No.: 202310163164.5, Publication No.: CN116280205A]. This device includes a connecting base, a lifting device that connects to the UAV via the connecting base, a support assembly positioned at the upper end of the lifting device, and a planetary gear assembly positioned at the center of the lower end of the lifting device. While the comparative patent can achieve autonomous grasping of cargo by the UAV, this invention further introduces stereo vision and inertial fusion positioning and mapping, a safe polygon feasible region constraint based on the distance field, and incorporates visibility and grasping direction constraints into trajectory optimization and performs phased planning for the tracking and grasping stages in dynamic target tracking and grasping tasks. Therefore, it surpasses the comparative patent in terms of safe planning and grasping stability under complex obstacle and moving target conditions. Summary of the Invention

[0006] In view of the problems existing in the prior art, the purpose of this invention is to provide a method for tracking and capturing dynamic targets on an unmanned aerial vehicle (UAV) at full load.

[0007] A method for tracking and capturing dynamic targets on an unmanned aerial vehicle (UAV) at full load, characterized by the following steps: S1 acquires color images and depth maps from a stereo camera, as well as inertial measurement data, and performs visual-inertial fusion to obtain the UAV pose and construct a local Euclidean symbolic distance field map; S2 performs target detection on the color image to obtain the bounding box and calculates the target's 3D position by combining the depth. It then uses an extended Kalman filter to obtain the target's position and velocity and estimates the grabbing direction. S3 predicts the target trajectory based on the target's position and velocity; S4 switches between the tracking and grasping phases based on the tracking distance and performs a segmented graph search on the predicted target trajectory to obtain a reference path. S5 optimizes its flight trajectory under constraints of obstacle avoidance, visibility, grabbing direction, and grabbing distance. The S6 controls the drone according to its flight trajectory and uses the deformable gripper to complete the grasping action based on the distance between the drone and the target.

[0008] Preferably, in step S1, the stereo camera outputs a color image and a depth map, the inertial measurement unit outputs acceleration and angular velocity, and the visual inertial odometry module fuses the camera and the inertial measurement unit to generate the UAV pose and odometry, and constructs a local Euclidean symbolic distance field map.

[0009] The above technical solutions enable the UAV to stably obtain its own pose and local Euclidean distance field map under full airborne conditions, providing a consistent spatiotemporal reference for subsequent target tracking, obstacle avoidance planning and trajectory optimization, thereby improving the real-time performance and safety of dynamic grasping tasks in complex environments.

[0010] Specifically, the color images and depth maps output by the stereo camera provide environmental geometric information, while the acceleration and angular velocity output by the inertial measurement unit provide short-term, high-frequency motion information. After being fused by visual-inertial odometry, the two can produce continuous and smooth pose and odometry outputs, reducing the drift risk of single vision in fast-moving or low-texture scenes. At the same time, the constructed local Euclidean symbolic distance field map can provide a basis for obstacle distance query and safe zone generation, enabling the planning process to have calculable obstacle avoidance basis.

[0011] In practical applications, this fusion positioning and mapping method can adapt to scenarios with dense obstacles and uncertain target movement. Even when frequent replanning is required, it can continuously output reliable pose and local map information, reducing problems such as infeasible paths and increased collision risks caused by unstable positioning or lack of environmental awareness. This lays the foundation for rapid approach and accurate alignment in the subsequent grasping stage.

[0012] Preferably, in step S2, the target bounding box B=(xmin,ymin,xmax,ymax) is obtained by detecting the color image based on the target detection network YOLOv8. The pixel depths within the bounding box are extracted from the depth map and the average value is taken as the three-dimensional position pc of the target in the camera coordinate system. The pc is converted into world coordinates pw by the extrinsic parameters provided by the visual inertial odometry. The target position and velocity are updated at a frequency of 30Hz using the extended Kalman filter state vector [pw,vw].

[0013] The above technical solution enables stable estimation of the three-dimensional position and velocity of dynamic targets, and maintains the continuity of target odometry when there is jitter or depth noise in visual detection, thereby improving the reliability of subsequent prediction and planning and increasing the success rate of grasping.

[0014] Specifically, after the target detection network outputs bounding boxes for the color image, it extracts the pixel depths within the bounding boxes from the depth map and takes the average value as the target's 3D position in the camera coordinate system, which can directly obtain the target's spatial ranging result. Then, it uses the extrinsic parameters provided by the visual inertial odometry to transform to the world coordinate system, so that the target position and the UAV pose are under the same coordinate reference, which is convenient for relative motion analysis. Subsequently, an extended Kalman filter is used to update the state vector [pw,vw] at a frequency of 30 Hz, which can effectively suppress the influence of detection noise and occasional abnormal measurements on velocity estimation, making the target velocity estimation smoother and more suitable for prediction.

[0015] In practical applications, this 3D positioning and filtering framework can still output a stable target state when the target moves rapidly, there are intermittent occlusions, or the background is complex and causes slight jumps in the detection box. This avoids drift of the planned endpoint or inaccurate judgment of the grasping timing caused by instantaneous errors, thereby improving the stable following capability in the tracking stage and the end alignment accuracy in the grasping stage.

[0016] Preferably, in step S2, for asymmetric targets, an initial graspable direction dg(0) is given, and based on the geometric constraint that "the target is fixed to the moving carrier and the angle between the grasping direction and the target velocity direction is constant", the angle Φ0 between the initial velocity v0 and the grasping direction dg0 is recorded, and the grasping direction dg(t) is inferred at any time based on the velocity v(t) estimated by the extended Kalman filter, such that ∠(dg(t),v(t))=Φ0.

[0017] The above technical solution enables the real-time inference of the graspable direction and the maintenance of geometric consistency with the target's motion direction when the target has an asymmetrical structure and the grasping requires a specific direction, thereby reducing grasping posture deviation and improving grasping reliability.

[0018] Specifically, by providing an initial graspable direction dg(0) a priori and recording the angle Φ0 between the initial velocity v0 and the grasping direction dg0, under the assumption that the target is fixed to the moving vehicle and the angle is constant, the current grasping direction dg(t) can be inferred from the filtered real-time velocity v(t), ensuring that ∠(dg(t), v(t)) = Φ0 holds. This method couples direction estimation with velocity estimation, enabling the grasping direction to adaptively update with the target's motion state without relying on additional complex identification of the target's attitude.

[0019] In practical applications, for targets with clear grasping surfaces or grasping orientation requirements, this direction inference mechanism enables the UAV to form the correct alignment trend in advance during the approach process, reducing secondary adjustments and time losses caused by orientation mismatch when the end is close. It is especially beneficial to maintain the determinism of the grasping action in dynamic scenarios with short grasping windows.

[0020] Preferably, in step S3, kinematic and dynamic prediction methods are used to predict the target in continuous time, and the predicted trajectory is sampled as a discrete position sequence.

[0021] The above technical solutions enable the acquisition of the target's future position sequence in advance even when the target is in continuous motion, allowing the planning to shift from "tracking the current point" to "approaching the future," thereby reducing tracking lag and improving the ability to seize opportunities during the grasping phase.

[0022] Specifically, kinematic and dynamic prediction methods are used to predict the target continuously over time, and the predicted trajectory is sampled as a discrete position sequence. This provides clear candidates for stage endpoints in graph search and offers a target reference that evolves over time for trajectory optimization. Through discretization, the characterization of the continuous motion trend is preserved, and it is also easy to align with the constraints and cost terms at the sampling time tk.

[0023] In practical applications, when the target speed changes or the motion path has curvature, the prediction and sampling sequence can enable the UAV to make preparations for speed and direction in advance, reduce the need for sudden maneuvers near the end, and make it easier to complete the sprint and gripper closure within a reasonable distance window during the grasping phase, thereby improving the overall mission success rate and execution smoothness.

[0024] Preferably, in step S4, a tracking distance dt is set; for each target prediction point zk, when the distance between the current position of the UAV and zk is greater than dt, a reachable and collision-free point is searched on a horizontal circle with zk as the center and radius dt as the endpoint of the time period map search; when the distance is less than dt, zk is used as the endpoint of the time period map search, and a segmented path is output as a reference node for trajectory optimization.

[0025] The above technical solution can maintain a reasonable following distance from the target during the tracking phase and ensure that the search endpoint is reachable. During the capture phase, it can quickly switch to an approach strategy with the target prediction point as the endpoint, thereby balancing stable following and end-capture efficiency, and reducing the path search failure rate.

[0026] Specifically, after setting the tracking distance dt, for each target prediction point zk, when the distance between the UAV and zk is greater than dt, a reachable and collision-free point is searched on a horizontal circle with zk as the center and radius dt as the search endpoint for that time period. This is equivalent to changing the endpoint from the "target point" to the "feasible following point", so that the path search does not have to force the crossing of obstacles or make excessive maneuvers. When the distance is less than dt, directly using zk as the endpoint can shorten the end path and improve the acquisition efficiency. The output segmented path serves as a reference node for trajectory optimization, making it easier for subsequent optimization to converge and satisfy constraints when starting from feasible initial values.

[0027] In practical applications, this segmented endpoint selection strategy can avoid the problems of "always hitting obstacles when chasing the target point" or "path being unreachable" when there are dense obstacles or narrow passages. At the same time, when approaching the grasping distance, it can switch to a more direct end approach in a timely manner, reducing detours and unnecessary path oscillations, thereby improving the success rate and real-time performance of online planning.

[0028] Preferably, in step S5: a safe polygon Si is generated along the reference path on the Euclidean symbolic distance field map, and S=Si, Si={x∈R3|Aix≤bi}; for sampling time tk, a visibility region Vk is defined based on the UAV position xk and the target position zk, and the angle between the vector xk-zk and the desired line-of-sight direction αk satisfies θV≤θmax as a visibility constraint, while a visibility cost JV=[max(cosθk-cosθV,0)] is introduced. 3 For asymmetric targets, a desired grasping direction βk is defined. When an obstacle exists along the βk direction, the deflection angle is determined by querying the Euclidean symbolic distance field or ray detection, and a safe angle offset is applied to βk. Let θD = ∠(xk-zk,βk), and introduce a grasping direction cost JD = (1-cosθD). 3 This incorporates visibility and grasping direction constraints into trajectory optimization.

[0029] The above technical solution can simultaneously characterize obstacle avoidance, visibility, and grasping direction requirements within the same planning framework, and safely deflect the desired grasping direction when there are obstacles interfering with it. This ensures that the target remains observable in complex environments and approaches the target in a suitable direction, thereby improving the stability of dynamic grasping.

[0030] Specifically, a safety polygon Si is generated along the reference path and expressed in the form Si={x∈R3|Aix≤bi}, which restricts the flight trajectory to a convex safety region, providing a computable geometric constraint for continuous obstacle avoidance. At sampling time tk, the visibility region Vk is defined based on xk and zk, and the included angle constraint θV≤θmax ensures that the target is within the camera's field of view. At the same time, a visibility cost JV=[max(cosθk-cosθV,0)] is introduced. 3 This results in a differentiable penalty for violating visibility; for asymmetric targets, a desired grasping direction βk is defined, and when there is an obstacle in the βk direction, the deflection angle is determined by querying the Euclidean symbolic distance field or ray detection, and a safe angle offset is performed. Then, let θD=∠(xk-zk,βk) and introduce a grasping direction cost JD=(1-cosθD). 3 This drives the trajectory to align with the target in the desired direction at the end stage.

[0031] In practical applications, the combination of "safe corridor plus soft constraint cost" can reduce the risk of failure caused by target going out of the field of view and alignment deviation while ensuring no collision. Especially when the grasping phase requires rapid approach and the field of view is easily lost, the visibility cost can make the trajectory maintain the observation conditions, and the grasping direction cost can make the approach direction meet the grasping requirements, thereby obtaining a higher dynamic grasping success rate and more stable execution performance.

[0032] Preferably, in step S6: the distance cost functions Ch(xh) and Cv(xv) in the horizontal and vertical directions are set respectively, and the open state θi and the fully closed state θf are preset for the gripper servo joint angle θ. When the distance d between the UAV and the target converges from a large value to dt, θdes(d) is controlled by linear interpolation. When d is greater than dc, it remains open. Between dc and dt, it is linearly closed according to the distance. When it is less than dt, it is fully closed to complete the grasping.

[0033] The above technical solution enables the flight end approach behavior and gripper action to be linked by distance, so that perception and flight stability are maintained at a long distance, and rapid capture and gripper closure are achieved at a close distance, thereby reducing grasping failure caused by premature closure or delayed closure.

[0034] Specifically, distance cost functions Ch(xh) and Cv(xv) are set for the horizontal and vertical directions respectively, which can differentiate the approach error in different directions, making the tracking phase more inclined to smooth following and the grasping phase more inclined to end-point sprint alignment. At the same time, the gripper servo joint angle θ is preset to open state θi and fully closed state θf, and θdes(d) is generated by linear interpolation according to the distance d between the UAV and the target. When d is greater than dc, it remains open, and between dc and dt, it is linearly closed according to the distance. When it is less than dt, it is fully closed, realizing synchronous convergence of gripper deformation and approach process.

[0035] In practical applications, this distance linkage control can avoid attitude fluctuations caused by airflow disturbances or accidental contact of the gripper in the early stages of approach. It can also ensure that the gripper action completes the closing in a timely and continuous manner when entering the grasping window, reducing discrete triggering and uncertainty in the end stage, making it easier for dynamic targets to be stably captured and grasped under short-term contact conditions.

[0036] Compared with the prior art, the present invention has the following advantages: 1. This invention integrates visual inertial positioning mapping, target state estimation and motion prediction, and phased path search and trajectory optimization on the airborne side, enabling UAVs to maintain a relatively coherent perception and planning link even when there are many obstacles or the target's motion changes, reducing reliance on external positioning or ground computing power, and thus making it easier to deploy and reuse in different operating environments.

[0037] 2. This invention considers constraints such as obstacle avoidance, target visibility, and grasping direction during the planning process, and controls the opening and closing of the gripper through distance linkage, making the coordination between the approach process and the grasping action more coordinated; when the target is asymmetrical or the grasping direction is restricted, it helps to reduce the instability caused by end-effector alignment deviation and improve the controllability of the grasping process. Attached Figure Description

[0038] Figure 1 This is a schematic flowchart of the present invention; Detailed Implementation

[0039] This invention proposes a fully airborne integrated aerial grasping system and method capable of autonomously tracking and grasping dynamic targets in complex environments. The system uses a multi-rotor flight platform as its carrier, integrating a deformable gripper, equipped with a stereo camera and inertial measurement unit. The onboard computing unit, with limited resources, achieves a closed-loop end-to-end system encompassing localization and mapping, target odometry acquisition, target prediction, two-stage planning for tracking and grasping, online trajectory optimization, and gripper deformation control.

[0040] (1) Sensing, self-localization and map building The stereo camera outputs color images and depth maps, while the inertial measurement unit outputs acceleration and angular velocity. The visual inertial odometry module integrates the camera and inertial measurement unit to generate UAV pose and odometry, and simultaneously constructs a local ESDF map for subsequent collision detection and safe zone generation.

[0041] (2) Target detection, target pose estimation, and orientation estimation The object detection network is used on the color image to output the object bounding box B=(x min ,y min ,x max ,y maxExtract the pixel depth within bounding box B of the depth map and filter out outliers to calculate the target's 3D position p in the camera coordinate system. c Then, the world coordinates p are obtained by transforming the extrinsic parameters provided by the visual inertial odometry. w Extended Kalman filtering is used to estimate the target's three-dimensional position and velocity, and the state vector is [p w ,v w It is updated at a frequency of approximately 30 Hz to filter out detection noise.

[0042] For asymmetric targets, the initial graspable direction d is provided prior. g (0). Based on the geometric constraint that "the target is fixed to the moving vehicle and the angle between the grasping direction and the velocity direction remains unchanged", the target velocity v0 and the initial grasping direction d are taken at the initial moment. g0 Calculate the included angle Φ0, and then at any subsequent time, deduce the current grasping direction d based on the velocity v(t) estimated by the extended Kalman filter. g (t), such that ∠(d) g (t),v t =Φ0.

[0043] (3) Target trajectory prediction A kinematic dynamics prediction method is introduced to provide the target predicted trajectory in continuous time and to sample and form a discrete target position sequence {zk} for subsequent path search and trajectory optimization.

[0044] (4) Two-stage hybrid path search of tracking and crawling Based on the horizontal distance and tracking distance d between the current position of the drone and the target position t In comparison, the automatic switching between the tracking and grasping phases is as follows: during the tracking phase, the target is approached smoothly while ensuring safety and visibility; during the grasping phase, the target is approached at higher speeds and the grasping direction is adjusted as the target approaches the grasping distance.

[0045] Specifically, for each target prediction point z k If the current drone position x cur With z k Distance d>d t Then in z k Let be the center and radius d. t Search for the most easily reachable collision-free point x on a horizontal circle. k As the endpoint of the graph search for this time period; if d≤d t Then directly use z k The endpoint is determined by a graph search performed on a 3D occupied grid or polygon graph to obtain segmented paths {x1,x2,…}, which serve as reference nodes for subsequent trajectory optimization.

[0046] (5) Construction of secure polygons and definition of visible and grabbing directions Generate a series of safety polygons S on the ESDF map along the reference path. i And represented as a set S i ={x∈R 3 |A i x≤b i}, to characterize the continuous collision constraints of the feasible trajectory space.

[0047] At each time sampling point k, based on the UAV position x k With target position z k Define the visibility region V k Ensure that the direction of the connection satisfies the geometric constraints of the camera's field of view; simultaneously define the grabbing direction region D. k This makes the direction of the connection align with the desired grabbing direction β. k The included angle should be as close to zero as possible.

[0048] (6) Two-stage unified trajectory optimization The UAV trajectory x(t) is represented by M segments of Nth-order polynomials, with coefficients c in each segment. i The segment duration is T. i .

[0049] Trajectory optimization aims at smoothness and time efficiency: the primary objective is to minimize the third derivative of the trajectory, i.e., the square integral of Jerk's equation; the secondary objective is to minimize the total time T while ensuring it is not shorter than the target prediction time T. p Constraints are imposed to avoid dynamic infeasibility.

[0050] Its unified optimization form, as given in the briefing, is a joint optimization of x(t) and T, with the objective including... It is subject to constraints including ρT, boundary derivative constraints, velocity constraints, acceleration constraints, collision constraints, visibility constraints, grabbing direction constraints, and grabbing distance constraints.

[0051] The dynamic constraint is given by |x'(t)|≤v max With |x''(t)|≤a max The collision constraint is x(t)∈S, and the visibility and grasping direction constraints are reflected at the sampling time as x(t)∈S. k )∈V k With x(t) k )∈D k The capture distance constraint is ||x(t) k )-z k ∥≤d t .

[0052] Using the MINCO trajectory class, the segmented duration T is... i Through the exponential parameter τ iMapping to a form that satisfies the total time constraint, the inequality constraint is transformed into an unconstrained optimization problem through a penalty function in the form of integral sampling, and then solved online using a large-scale optimization algorithm.

[0053] (7) Calculation method of visibility soft constraint cost and grasping direction soft constraint cost Regarding visibility constraints, the observation cone is obtained by defining the camera's field of view and mounting offset angle; at sampling time t k According to the target position z k With respect to the desired line of sight α k Calculate vector x k -z k With α k The included angle satisfies θ V ≤θ max The requirements, and through the cost J V =max(cosθ k -cosθ V ,0) 3 Implement soft constraints.

[0054] Regarding the grasping direction constraint, for asymmetric targets, the desired grasping direction is set to β. k When along β k When there is an obstacle in the direction, adjust β at a safe angle. k Offset to avoid obstacles; let θ D =∠(x k -z k ,β k Its ideal value is zero, and the design cost J is zero. D =(1-cosθ D ) 3 To make the trajectory follow β as much as possible at the moment of capture. k The direction is approaching the target.

[0055] (8) Distance-dependent grasping behavior design and gripper deformation control Design distance cost functions C for the horizontal and vertical directions respectively. h (x h ) and C v (x v This is used to switch between a smooth approach during the long-distance phase and a sprint grabbing behavior during the close-distance phase.

[0056] For the gripper servo joint angle θ, the preset open state angle θ i With the angle of complete closure θ f When the distance d between the drone and the target decreases from a larger value to d t During convergence, the desired angle θ is controlled by linear interpolation. des (d): When d>d c Keep it open, in dc to d t The distance between them converges linearly, when d <d t The grab is completed when the device is fully closed.

[0057] Example The system verifies dynamic target tracking and grasping in a dense obstacle environment. The average distance between obstacles in the environment is approximately 4 meters, and the target is fixed to a moving vehicle and moves in a straight line or curve. The system is equipped with a stereo camera and an inertial measurement unit and operates fully autonomously on an onboard computing unit. Target detection and pose estimation are output at approximately 30 Hz for target odometry.

[0058] I. Target Odometry Acquisition and Online Direction Inference The stereo camera outputs a color image with a resolution of 640×480. The bounding box B=(x) is obtained after detection. min ,y min ,x max ,y max The target's position p in the camera frame is obtained by taking the effective depth points within the bounding box of the depth map and calculating the average value. c Then switch to the world system p w With extended Kalman filter state [p] w ,v w ] Perform filtering and update to obtain the target velocity estimate v(t).

[0059] For asymmetric targets, given an initial grasping direction d g (0). Take v0 and d at the initial time. g0 Calculate the included angle Φ0, and update the grasping direction d in each subsequent planning cycle based on the newly estimated velocity v(t). g (t), such that ∠(d) g (t),v t =Φ0, thus ensuring that the geometric relationship between the grasping direction and the target movement direction remains consistent.

[0060] II. Two-stage hybrid path search and safe feasible region construction To track distance d t As a stage switching threshold, when d>d t Then, the tracking phase begins, and for each predicted target point z... k The search along its horizontal circumference yields a collision-free point as the endpoint of the segment; when d ≤ d t When entering the crawling phase, directly use z k Search for the endpoint of the segment to obtain the segment reference path {x1,x2,…}.

[0061] Generate a safe polygon sequence S on the ESDF map along the reference path. i and with S i={x∈R 3 |A i x≤b i The} represents the feasible region of continuous collisions, providing a hard constraint basis for subsequent continuous time trajectory optimization.

[0062] III. Visibility and grab direction soft constraint calculations are incorporated into unified optimization. For each sampling time t k According to the target position z k With respect to the desired line of sight α k Calculate vector x k -z k With α k The included angle satisfies θ V ≤θ max The field of view is limited, and J is used. V =max(cosθ k -cosθ V ,0) 3 It is added as a differentiable penalty term to the optimization objective to reduce the risk of failure caused by the target going out of the field of view.

[0063] Simultaneously, θ is calculated for the asymmetric target. D =∠(x k -z k ,β k ) and J D =(1-cosθ D ) 3 Penalize deviation, causing the grasping end to move in the desired direction β. k Aim at the target.

[0064] IV. MINCO Online Trajectory Optimization and Real-time Replanning Using the unified optimization form described above, x(t) and T are jointly optimized. The objective includes a Jerk smoothing term and a time term ρT, while simultaneously satisfying T≥T p 、 |x'(t)|≤v max 、 |x''(t)|≤a max x(t)∈S, x(t) k )∈V k 、x(t k )∈D k With ∥x(t) k )-z k ∥≤d t Constraints, etc.

[0065] Segment duration T i Through the exponential parameter τ i The mapping process incorporates inequality constraints into the unconstrained optimization using an integral sampling penalty term, which is then solved online by the onboard unit. The average replanning time per cycle is approximately 4.21 ms, meeting real-time requirements.

[0066] V. Execution of Distance-Related Gripper Deformation Control During trajectory execution, the desired gripper servo angle θ is calculated based on the current distance d between the UAV and the target. des (d). When d>d c Keep open, when d c ≤d≤d t Linear convergence, when d <d t It is fully closed to enable adaptive and coordinated grasping actions in conjunction with the flight approach process.

[0067] VI. Experimental Phenomena and Results Citation In the aforementioned dense obstacle environment, the system was able to maintain obstacle avoidance and complete the capture during both the tracking and capture phases, with the maximum flight speed reaching 1.44 m / s.

[0068] Under the condition that the target's average speed is about 0.63 m / s and the maximum speed is 1.03 m / s, the drone's average speed is about 1.0 m / s and the maximum speed is 1.44 m / s, and the tracking and capture success rate is about 71%.

[0069] The above description is merely a preferred embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any equivalent substitutions or modifications made by those skilled in the art within the scope of the technology disclosed in the present invention, based on the technical solution and inventive concept of the present invention, should be covered within the scope of protection of the present invention.

Claims

1. A method for tracking and capturing dynamic targets entirely onboard by an unmanned aerial vehicle (UAV), characterized in that, Includes the following steps: S1 acquires color images and depth maps from a stereo camera, as well as inertial measurement data, and performs visual-inertial fusion to obtain the UAV pose and construct a local Euclidean symbolic distance field map; S2 performs target detection on the color image to obtain the bounding box and calculates the target's 3D position by combining the depth. It then uses an extended Kalman filter to obtain the target's position and velocity and estimates the grabbing direction. S3 predicts the target trajectory based on the target's position and velocity; S4 switches between the tracking and grasping phases based on the tracking distance and performs a segmented graph search on the predicted target trajectory to obtain a reference path. S5 optimizes its flight trajectory under constraints of obstacle avoidance, visibility, grabbing direction, and grabbing distance. The S6 controls the drone according to its flight trajectory and uses the deformable gripper to complete the grasping action based on the distance between the drone and the target.

2. The method according to claim 1, characterized in that, In step S1, the stereo camera outputs a color image and a depth map, the inertial measurement unit outputs acceleration and angular velocity, and the visual inertial odometry module fuses the camera and the inertial measurement unit to generate the UAV pose and odometry, and constructs a local Euclidean symbolic distance field map.

3. The method according to claim 1, characterized in that, In step S2, the target bounding box B=(xmin,ymin,xmax,ymax) is obtained by detecting the color image based on the target detection network YOLOv8. The pixel depths within the bounding box are extracted from the depth map and the average value is taken as the three-dimensional position pc of the target in the camera coordinate system. The pc is converted into world coordinates pw by the extrinsic parameters provided by the visual inertial odometry. The target position and velocity are updated at a frequency of 30Hz using the extended Kalman filter state vector [pw,vw].

4. The method according to claim 1, characterized in that, In step S2, for asymmetric targets, an initial graspable direction dg(0) is given, and based on the geometric constraint that "the target is fixed to the moving carrier and the angle between the grasping direction and the target velocity direction is constant", the angle Φ0 between the initial velocity v0 and the grasping direction dg0 is recorded, and the grasping direction dg(t) is inferred at any time based on the velocity v(t) estimated by the extended Kalman filter, such that ∠(dg(t),v(t))=Φ0.

5. The method according to claim 1, characterized in that, In step S3, kinematic and dynamic prediction methods are used to predict the target in continuous time, and the predicted trajectory is sampled as a discrete position sequence.

6. The method according to claim 1, characterized in that, In step S4, a tracking distance dt is set. For each target prediction point zk, when the distance between the current position of the UAV and zk is greater than dt, a reachable and collision-free point is searched on a horizontal circle with zk as the center and radius dt as the endpoint of the time period map search. When the distance is less than dt, zk is used as the endpoint of the time period map search, and a segmented path is output as a reference node for trajectory optimization.

7. The method according to claim 1, characterized in that, In step S5: a safe polygon Si is generated along the reference path on the Euclidean symbolic distance field map, and S=Si, Si={x∈R3|Aix≤bi}; for sampling time tk, a visibility region Vk is defined based on the UAV position xk and the target position zk, and the angle between the vector xk-zk and the desired line-of-sight direction αk satisfies θV≤θmax as a visibility constraint, while a visibility cost JV=[max(cosθk-cosθV,0)] is introduced. 3 For asymmetric targets, a desired grasping direction βk is defined. When an obstacle exists along the βk direction, the deflection angle is determined by querying the Euclidean symbolic distance field or ray detection, and a safe angle offset is applied to βk. Let θD = ∠(xk-zk,βk), and introduce a grasping direction cost JD = (1-cosθD). 3 This incorporates visibility and grasping direction constraints into trajectory optimization.

8. The method according to claim 1, characterized in that, In step S6: the distance cost functions Ch(xh) and Cv(xv) in the horizontal and vertical directions are set respectively, and the open state θi and the fully closed state θf are preset for the gripper servo joint angle θ. When the distance d between the UAV and the target converges from a large value to dt, θdes(d) is controlled by linear interpolation. When d is greater than dc, it remains open. Between dc and dt, it is linearly closed according to the distance. When it is less than dt, it is fully closed to complete the grasping.

Images