Target-oriented tracking method for unmanned aerial vehicle trajectory planning

Abstract

The application provides a target tracking-oriented unmanned aerial vehicle trajectory planning method, and belongs to the technical field of unmanned aerial vehicle control and autonomous trajectory planning, and the method comprises the following steps: S1. state and environment information acquisition; step S2. target trajectory prediction; step S3. MPC trajectory planning model construction; step S4. target function construction and optimization solution; step S5. trajectory generation and instruction execution; step S6. rolling horizon update and cycle. The application effectively reduces the risk of target loss caused by obstacles or high-speed target maneuvering, and improves the completion rate and robustness of the tracking task.

Description

Technical Field

[0001] This invention relates to the fields of unmanned aerial vehicle (UAV) control technology and autonomous trajectory planning technology, and in particular to a UAV trajectory planning method for target tracking. Background Technology

[0002] Autonomous target tracking is one of the key technologies for drones in fields such as aerial photography, inspection, security, and search and rescue. With the continuous expansion of drone application scenarios, continuous and stable visual tracking of moving targets has become an important capability supporting drones in autonomously performing tasks.

[0003] Currently, various technical solutions have emerged for the trajectory planning problem in the target tracking process of unmanned aerial vehicles (UAVs). Based on their different planning mechanisms, they can be roughly divided into three categories: reactive planning based on real-time perception, hierarchical planning based on path search and trajectory optimization, and decision planning based on learning.

[0004] Real-time perception-based reactive planning methods generate speed commands directly based on the currently perceived target position and obstacle information, making real-time decisions, such as by constructing a visibility field. While these methods have low computational cost and fast response times, they rely solely on current information and do not anticipate the target's future motion. This can lead to delays in decision-making, causing the target to leave the drone's field of view, especially when the target is maneuvering rapidly or surrounded by complex obstacles. Furthermore, these methods struggle to proactively avoid potential line-of-sight obstruction risks.

[0005] Hierarchical planning methods based on path search and trajectory optimization typically employ a two-stage strategy of "search first, then optimize": First, an initial geometric path that meets obstacle avoidance requirements is generated in the environment map using an A* isograph search algorithm. Then, the initial path is smoothed using methods such as B-spline or polynomial curve fitting to satisfy the kinematic constraints of the UAV. Some schemes introduce concepts such as field-of-view skeletons or safe flight corridors in the search stage to take into account the target visibility requirements in the path planning stage; other schemes introduce occlusion effect measurements and observation angle constraints in the trajectory optimization stage to further improve the field-of-view maintenance capability during tracking.

[0006] However, the aforementioned hierarchical planning method still has shortcomings in practical applications. On the one hand, the graph search stage usually relies on a pre-built global environment map, which has poor adaptability in dynamic or unknown environments, and the computational cost of the search increases significantly with the scale of the environment. On the other hand, existing visible area modeling often uses two-dimensional sector approximation or simplified geometric models, making it difficult to accurately assess the occlusion effect of obstacles in three-dimensional space on the line of sight between the UAV and the target, resulting in the field of view constraint easily failing in complex three-dimensional environments.

[0007] Furthermore, the two-stage paradigm of "search first, optimize later" has inherent flaws: in satisfying kinematic constraints and dynamic feasibility, the back-end trajectory optimization may deviate from the initial path obtained by the front-end search, thereby affecting the safety and visibility guaranteed in the path planning stage and leading to a decrease in the stability of the final generated trajectory. Although some solutions introduce new visibility constraints, their constraint effects are quite sensitive to preset thresholds, and their robustness needs to be improved.

[0008] Learning-based decision-making and planning methods, such as the UAV ground target tracking and obstacle avoidance planning method based on deep reinforcement learning disclosed in patent document CN117707207A, guide the UAV to maintain target visibility by setting a visibility reward function in the Markov decision model. However, deep reinforcement learning methods have inherent problems such as training relying on a large amount of sample data, limited generalization ability to unknown scenarios, and poor interpretability of the decision-making process. Their robustness and reliability in complex dynamic environments still cannot meet the needs of practical applications.

[0009] As mentioned above, existing UAV target tracking trajectory planning technologies still face the following pressing technical challenges: 1. Visibility maintenance and real-time planning are difficult to coordinate. The two-stage "search first, optimize later" approach can lead to a decrease in the stability of field of view maintenance because the back-end trajectory may deviate from the initial path. The approach using real-time reactive planning lacks prediction of the target's future movement and has insufficient forward-looking decision-making, making it easy to lose the target when it moves rapidly.

[0010] 2. Insufficient accuracy in 3D modeling of field of view constraints. Existing visible area modeling is mostly based on 2D approximations or geometric simplifications, which cannot accurately assess the occlusion of the line of sight by obstacles in 3D space. As a result, field of view constraints are difficult to play an effective role in complex 3D obstacle environments.

[0011] 3. Poor environmental adaptability of control parameters. Existing methods mostly use fixed control parameters, which cannot be dynamically adjusted according to changes in the target's speed and the congestion of the surrounding environment, making it difficult to achieve an effective balance between high-speed tracking and safe obstacle avoidance.

[0012] 4. Insufficient recapture capability after target loss. Existing methods typically lack effective search and recapture mechanisms after a target is briefly lost due to maneuvering or obstruction, making it difficult to guarantee the continuity of the tracking task. Summary of the Invention

[0013] This invention provides a UAV trajectory planning method for target tracking, which overcomes the problems of poor coordination between visibility maintenance and real-time planning, poor parameter adaptability, and weak re-acquisition capability in the prior art. It effectively reduces the risk of target loss due to obstacle obstruction or high-speed target maneuvering, and improves the completion rate and robustness of tracking tasks.

[0014] To achieve the above objectives, the present invention adopts the following technical solution: UAV trajectory planning methods for target tracking include: Step S1: Real-time acquisition of the current state information of the UAV, the current state information of the moving target, and the Euclidean symbolic distance field of the surrounding environment; the current state information of the UAV includes three-dimensional position, three-dimensional velocity, and yaw angle; the current state information of the moving target includes three-dimensional position and three-dimensional velocity; Step S2: Based on the current state information of the moving target, a constant velocity model is used to predict the target's predicted trajectory for a preset number of N time steps in the future, with each time step corresponding to the target's predicted position at a prediction time. Step S3: Construct a trajectory planning model based on model predictive control. Use the current state information of the UAV as the initial state and the UAV acceleration sequence over the next N time steps as optimization variables. Set the following constraints: kinematic constraints recursively derived from the current state information of the UAV; obstacle avoidance safety constraints based on the Euclidean sign distance field; target distance holding constraints to maintain the distance between the UAV and the target; field-of-view holding constraints to keep the target within the UAV's field of view; acceleration smoothing constraints to constrain the rate of change of acceleration; and tracking orientation guidance constraints to guide the UAV to approach the target's velocity direction directly behind it. The field-of-view holding constraints evaluate the occlusion on the line connecting the UAV and the target based on the yaw angle, a preset square pyramid field-of-view model, and a spherical region fitting method. The tracking orientation guidance constraints define an ideal following point, located at a fixed distance offset from the target's predicted position along the opposite direction of the target velocity vector on the predicted target trajectory, and constrain the UAV's position to approach the ideal following point. Step S4: Construct an objective function that includes the kinematic constraints, obstacle avoidance safety constraints, target distance maintenance constraints, field of view maintenance constraints, acceleration smoothing constraints, and tracking orientation guidance constraints, and use a sequential quadratic programming solver to solve the optimization variables in the rolling time domain to obtain the optimal acceleration sequence; Step S5: Generate a sequence of trajectory points for the UAV over the next N time steps based on the optimal acceleration sequence and the current state information of the UAV, and execute the control command for only the first time step. Step S6: In the next control cycle, return to step S1 and repeat steps S1 to S5.

[0015] In this specification, the objective function consists of multiple weighted terms, including: a first weighted term characterizing acceleration smoothness, a second weighted term characterizing the safe distance between the UAV and the obstacle, a third weighted term characterizing the deviation between the actual distance and the expected distance between the UAV and the target, a fourth weighted term characterizing the angular deviation of the target in the UAV's field of view, a fifth weighted term characterizing the rate of change of acceleration in adjacent time steps, and a sixth weighted term characterizing the deviation between the UAV's position and the ideal following point; each weighted term is multiplied by a preset weighting coefficient and then summed to form the objective function.

[0016] In this specification, the method for constructing the field-of-view constraint is as follows: construct the regular square pyramid field-of-view model with the UAV as the vertex; set multiple spherical regions on the line connecting the UAV and the target, the center position and radius of each spherical region are jointly determined by the three-dimensional position, the preset field-of-view angle and the preset interval parameter; obtain the signed distance value from the center of each spherical region to the nearest obstacle by querying the Euclidean signed distance field; when the signed distance value is less than the radius of the corresponding spherical region, it is determined that there is occlusion at the location of the spherical region, and a differentiable cost function is constructed accordingly as the field-of-view constraint.

[0017] In this specification, the fixed distance in the tracking orientation guidance constraint is the average of the preset minimum tracking distance and the preset maximum tracking distance; the ideal following point is located at the position of the target predicted position offset by the fixed distance in the opposite direction of the target velocity vector; the tracking orientation guidance constraint is implemented by penalizing the Euclidean distance between the actual position of the UAV and the ideal following point.

[0018] This specification also includes a parameter adaptive adjustment step: dynamically adjusting the minimum tracking distance, maximum tracking distance, and the weight of the third weighting term in the target distance maintenance constraint according to the real-time speed of the moving target; and dynamically adjusting the prediction step number N according to the Euclidean symbolic distance field value of the current position of the UAV.

[0019] In this specification, the dynamic adjustment of the minimum tracking distance, maximum tracking distance, and the weight of the third weighting term based on the real-time speed of the moving target specifically includes: calculating the speed of the moving target and defining a dimensionless speed factor; when the speed of the moving target increases, linearly reducing the minimum tracking distance and the maximum tracking distance, and linearly increasing the weight of the third weighting term; when the speed of the moving target decreases, linearly restoring the weights of the minimum tracking distance, the maximum tracking distance, and the third weighting term to their respective baseline values.

[0020] In this specification, the dynamic adjustment of the prediction step number N based on the Euclidean symbolic distance field value of the UAV's current position specifically includes: obtaining the Euclidean symbolic distance field value of the UAV's current position; calculating a congestion factor based on the Euclidean symbolic distance field value and a preset congestion determination distance; when the Euclidean symbolic distance field value is less than the congestion determination distance, linearly increasing the prediction step number N based on the congestion factor, and the value of N after the increase does not exceed a preset maximum prediction step number; and performing a first-order low-pass filter smoothing process on the Euclidean symbolic distance field value to avoid frequent jumps in the prediction step number.

[0021] This specification also includes a target reacquisition step: When a moving target is lost from the UAV's field of view, the UAV first flies to the target's last known position at its maximum safe speed. The last known position is the three-dimensional position of the moving target acquired last time before it was lost. After reaching the last known position, the UAV adjusts its yaw angle to point to a short-term estimated position calculated based on the target's last known speed and direction of motion, and flies forward in the direction indicated by the yaw angle to perform detection. If the target is still not detected, the UAV switches to flying along the target's last known direction of motion and periodically performs yaw maneuvers during flight to expand the search field of view. At the same time, it uses particle filtering to generate multiple candidate target positions to guide the search. If the target is not reacquisitioned within a preset time threshold, the tracking is deemed to have failed.

[0022] In this specification, the kinematic constraints of the UAV include: the acceleration amplitude at each time step does not exceed the preset maximum acceleration, the velocity amplitude at each time step does not exceed the preset maximum velocity, the yaw angle angular velocity amplitude at each time step does not exceed the preset maximum yaw angle angular velocity, and the rate of change of acceleration between adjacent time steps does not exceed the preset maximum rate of change of acceleration.

[0023] In this specification, when the sequential quadratic programming solver is used to solve the optimization variables in step S4, a hot start method is adopted: the acceleration of the first time step in the optimal acceleration sequence obtained in the previous control cycle is removed, and the remaining acceleration sequence is shifted forward by one time step in sequence. Then, the shifted sequence is used as the initial guess value of the sequential quadratic programming solver in the current control cycle.

[0024] In summary, the present invention has at least the following beneficial effects: This invention abandons the two-layer planning paradigm of "search first, then optimize" and directly adopts MPC as a unified planning framework. Compared with existing methods, this invention can still generate stable and smooth following trajectories when the target is maneuvering at high speed, significantly improving the robustness and tracking continuity of the system.

[0025] By explicitly embedding the field-of-view constraint into the MPC optimization model, a regular square pyramid geometry model is used to approximate the actual field of view of the UAV, and a spherical region fitting method is employed to evaluate the degree of obstacle occlusion on the line of sight, thus constructing a differentiable field-of-view constraint cost function. This enables the UAV to effectively avoid the risk of target occlusion in complex obstacle environments, keeping the target always within the effective field of view.

[0026] The speed-driven distance / weight adjustment mechanism and congestion-driven prediction time-domain adjustment mechanism proposed in this invention enable the MPC controller parameters to dynamically adjust according to the target speed and environmental congestion level. When the target is maneuvering at high speed, the tracking distance is automatically tightened and the tracking weight is increased to ensure close following; when the environment is congested, the prediction time domain is automatically extended to improve obstacle avoidance look-ahead capability. This decoupled design allows the UAV to maintain excellent tracking performance in different scenarios, while avoiding the computational burden caused by unnecessary expansion of the prediction steps.

[0027] This invention designs a complete four-stage target re-acquisition scheme, including emergency approach to the lost point, priority pointing to the predicted position, flight search along the last direction of motion, and multi-angle dynamic search. Compared with existing technologies that rely solely on prediction models for recovery, this scheme can quickly re-lock the target's position after it is temporarily lost due to sudden velocity changes or occlusion, effectively reducing the risk of tracking mission failure caused by brief occlusion.

[0028] This invention innovatively designs an ideal following point guidance term based on velocity direction, guiding the UAV to actively approach the optimal observation position directly behind the target. This guidance term works in conjunction with angle constraints to not only ensure that the target is located near the center of the field of view, but also to place the UAV in an observation position with minimal obstruction and the most complete target outline. Especially when the target is turning or passing through obstacles, it can effectively reduce the probability of loss of field of view caused by corner obstruction. Attached Figure Description

[0029] Figure 1 This is a schematic diagram of the framework of the UAV trajectory planning method for target tracking involved in this invention.

[0030] Figure 2 This is a schematic diagram of the visible area fitted to the spherical region involved in this invention.

[0031] Figure 3 This is a schematic diagram illustrating the effect of the tracking relative position on the field of view involved in this invention.

[0032] Figure 4 This is a schematic diagram of the field of view recapture involved in this invention.

[0033] Figure 5 This is a schematic diagram of the target tracking simulation environment and trajectory involved in Experiment 1 of this invention.

[0034] Figure 6 This is a schematic diagram of the target tracking simulation environment and trajectory involved in Experiment 2 of this invention.

[0035] Figure 7 This is a schematic diagram of the target tracking simulation environment and trajectory involved in Experiment 3 of this invention.

[0036] Figure 8 This is a schematic diagram of the target tracking simulation environment and trajectory involved in Experiment 4 of this invention.

[0037] Figure 9 This is a schematic diagram of the tracking performance curve of Experiment 1 involved in this invention.

[0038] Figure 10 This is a schematic diagram of the tracking performance curve in Experiment 2 involved in this invention.

[0039] Figure 11 This is a schematic diagram of the tracking performance curve in Experiment 3 involved in this invention.

[0040] Figure 12 This is a schematic diagram of the tracking performance curve in Experiment 4 involved in this invention.

[0041] Figure 13 This is a schematic diagram illustrating the effect of unguided tracking involved in this invention.

[0042] Figure 14 This is a schematic diagram illustrating the guided item tracking effect involved in this invention. Detailed Implementation

[0043] The embodiments of the present invention will now be described in detail with reference to the accompanying drawings.

[0044] like Figure 1 As shown, this embodiment provides a UAV trajectory planning method for target tracking, including: Step S1: Real-time acquisition of the current state information of the UAV, the current state information of the moving target, and the Euclidean symbolic distance field of the surrounding environment; the current state information of the UAV includes three-dimensional position, three-dimensional velocity, and yaw angle; the current state information of the moving target includes three-dimensional position and three-dimensional velocity; Step S2: Based on the current state information of the moving target, a constant velocity model is used to predict the target's predicted trajectory for a preset number of N time steps in the future, with each time step corresponding to the target's predicted position at a prediction time. Step S3: Construct a trajectory planning model based on model predictive control. Use the current state information of the UAV as the initial state and the UAV acceleration sequence over the next N time steps as optimization variables. Set the following constraints: kinematic constraints recursively derived from the current state information of the UAV; obstacle avoidance safety constraints based on the Euclidean sign distance field; target distance holding constraints to maintain the distance between the UAV and the target; field-of-view holding constraints to keep the target within the UAV's field of view; acceleration smoothing constraints to constrain the rate of change of acceleration; and tracking orientation guidance constraints to guide the UAV to approach the target's velocity direction directly behind it. The field-of-view holding constraints evaluate the occlusion on the line connecting the UAV and the target based on the yaw angle, a preset square pyramid field-of-view model, and a spherical region fitting method. The tracking orientation guidance constraints define an ideal following point, located at a fixed distance offset from the target's predicted position along the opposite direction of the target velocity vector on the predicted target trajectory, and constrain the UAV's position to approach the ideal following point. Step S4: Construct an objective function that includes the kinematic constraints, obstacle avoidance safety constraints, target distance maintenance constraints, field of view maintenance constraints, acceleration smoothing constraints, and tracking orientation guidance constraints, and use a sequential quadratic programming solver to solve the optimization variables in the rolling time domain to obtain the optimal acceleration sequence; Step S5: Generate a sequence of trajectory points for the UAV over the next N time steps based on the optimal acceleration sequence and the current state information of the UAV, and execute the control command for only the first time step. Step S6: In the next control cycle, return to step S1 and repeat steps S1 to S5.

[0045] In some embodiments, the objective function consists of multiple weighted terms, including: a first weighted term characterizing acceleration smoothness, a second weighted term characterizing the safe distance between the UAV and the obstacle, a third weighted term characterizing the deviation between the actual distance and the expected distance between the UAV and the target, a fourth weighted term characterizing the angular deviation of the target in the UAV's field of view, a fifth weighted term characterizing the rate of change of acceleration in adjacent time steps, and a sixth weighted term characterizing the deviation between the UAV's position and the ideal following point; each weighted term is multiplied by a preset weighting coefficient and then summed to form the objective function.

[0046] In some embodiments, the method for constructing the field-of-view constraint is as follows: constructing the regular square pyramid field-of-view model with the UAV as the vertex; setting multiple spherical regions on the line connecting the UAV and the target, wherein the center position and radius of each spherical region are jointly determined by the three-dimensional position, the preset field-of-view angle, and the preset interval parameter; obtaining the signed distance value from the center of each spherical region to the nearest obstacle by querying the Euclidean signed distance field; when the signed distance value is less than the radius of the corresponding spherical region, determining that there is occlusion at the location of the spherical region, and constructing a differentiable cost function as the field-of-view constraint accordingly.

[0047] In some embodiments, the fixed distance in the tracking orientation guidance constraint is the average of a preset minimum tracking distance and a preset maximum tracking distance; the ideal following point is located at the position where the target predicted position is offset by the fixed distance in the opposite direction of the target velocity vector; the tracking orientation guidance constraint is implemented by penalizing the Euclidean distance between the actual position of the UAV and the ideal following point.

[0048] In some embodiments, the method further includes a parameter adaptive adjustment step: dynamically adjusting the minimum tracking distance, maximum tracking distance, and the weight of the third weighting term in the target distance maintenance constraint according to the real-time speed of the moving target; and simultaneously, dynamically adjusting the prediction step number N according to the Euclidean symbolic distance field value of the current position of the UAV.

[0049] In some embodiments, dynamically adjusting the minimum tracking distance, maximum tracking distance, and the weight of the third weighting term based on the real-time speed of the moving target specifically includes: calculating the speed of the moving target and defining a dimensionless speed factor; when the speed of the moving target increases, linearly reducing the minimum tracking distance and the maximum tracking distance, and linearly increasing the weight of the third weighting term; when the speed of the moving target decreases, linearly restoring the weights of the minimum tracking distance, the maximum tracking distance, and the third weighting term to their respective baseline values.

[0050] In some embodiments, dynamically adjusting the prediction step number N based on the Euclidean symbolic distance field value of the UAV's current position specifically includes: obtaining the Euclidean symbolic distance field value of the UAV's current position; calculating a congestion factor based on the Euclidean symbolic distance field value and a preset congestion determination distance; when the Euclidean symbolic distance field value is less than the congestion determination distance, linearly increasing the prediction step number N based on the congestion factor, and the value of N after the increase does not exceed a preset maximum prediction step number; and performing a first-order low-pass filter smoothing process on the Euclidean symbolic distance field value to avoid frequent jumps in the prediction step number.

[0051] In some embodiments, a target reacquisition step is further included: when a moving target is lost from the UAV's field of view, the UAV first flies to the target's last known position at the maximum safe speed, where the last known position is the three-dimensional position of the moving target acquired before it was lost; after reaching the target's last known position, the UAV adjusts its yaw angle to point to a short-term estimated position calculated based on the target's last known speed and direction of motion, and flies forward in the direction indicated by the yaw angle to perform detection; if the target is still not detected, the UAV switches to flying along the target's last known direction of motion, and periodically performs yaw maneuvers during flight to expand the search field of view, while using particle filtering to generate multiple candidate target positions to guide the search; if the target is not reacquisitioned within a preset time threshold, the tracking is deemed to have failed.

[0052] In some embodiments, the kinematic constraints of the UAV include: the acceleration amplitude at each time step does not exceed a preset maximum acceleration, the velocity amplitude at each time step does not exceed a preset maximum velocity, the yaw angle angular velocity amplitude at each time step does not exceed a preset maximum yaw angle angular velocity, and the rate of change of acceleration between adjacent time steps does not exceed a preset maximum rate of change of acceleration.

[0053] In some embodiments, when using a sequential quadratic programming solver to solve for the optimization variables in step S4, a hot start method is adopted: the acceleration of the first time step in the optimal acceleration sequence obtained from the previous control cycle is removed, and the remaining acceleration sequence is shifted forward by one time step in sequence, and then the shifted sequence is used as the initial guess value of the sequential quadratic programming solver for the current control cycle.

[0054] The technical concept of this invention is as follows: In this invention, the trajectory planning for UAV target tracking is constructed as a conditional optimization problem. This problem, based on surrounding environmental information and the target's current state, seeks a safe, smooth (kinematically constrained) flight trajectory (position and velocity over time) that maintains target visibility within a desired distance range, thereby achieving stable target tracking. During the tracking process, this invention makes the following assumptions: 1. The three-dimensional position, three-dimensional velocity, and yaw angle of the UAV can be obtained in real time, and the motion of the UAV is described by a simplified kinematic model that includes the three-dimensional translational state and the yaw angle state.

[0055] 2. When the moving target is within the effective perception range of the UAV, the three-dimensional position and three-dimensional velocity of the moving target can be obtained in real time.

[0056] 3. The future short-term motion state of a moving target can be predicted using a constant velocity model based on its current motion state.

[0057] 4. Obstacles in the environment are considered stationary within a single prediction time domain.

[0058] 5. The local environment around the UAV is obtained through real-time mapping, and the obstacle occupancy point or obstacle surface point can be extracted for calculating the Euclidean distance of the nearest obstacle.

[0059] The framework diagram of the UAV trajectory planning method for target tracking proposed in this invention is as follows: Figure 1 As shown: This framework is based on model predictive control (MPC), which integrates target motion information obtained by UAV sensors with the Euclidean symbolic distance field of the environment. It solves the UAV trajectory by rolling optimization of the objective function with field of view constraints, thereby realizing field of view tracking and obstacle avoidance trajectory planning for moving targets.

[0060] In this invention, the trajectory planning problem of UAVs is fitted into a mathematical function optimization problem. A series of three-dimensional coordinate points and the dynamic relationship between each point in the time dimension are used as the UAV trajectory. Based on the requirements of trajectory tracking, an objective function is constructed. Each constraint is mathematically fitted as a polynomial for each part of the objective function, and then the optimal solution is obtained.

[0061] 1. Unified definition of time variables and forecast indexes: To avoid confusion between continuous-time variables, discrete prediction steps, and target identifiers, this embodiment uniformly stipulates: Represents a continuous real-time variable. This indicates the actual time corresponding to the current control cycle. This represents the index of the discrete prediction step in model predictive control. Indicates a fixed discrete sampling period. This indicates the number of prediction steps used in the current control cycle.

[0062] No. The actual prediction time corresponding to each prediction step for: (1) In the formula: For continuous real-time variables; This refers to the actual moment corresponding to the current control cycle; Index for discrete prediction steps; For fixed discrete sampling period; The number of prediction steps used for the current control cycle.

[0063] 2. Moving target trajectory prediction: Suppose the moving target is at the current time The position and velocity are respectively and Then the moving target is in the 1st month. The positions of the predicted times are: (2) In the formula: For the moving target in the first The location of the predicted time; For the moving target at the current moment Location; For the moving target at the current moment speed; Index for discrete prediction steps; For fixed discrete sampling period.

[0064] The target predicted location sequence is represented as follows: (3) In the formula: Predict the location sequence for the target; For the moving target in the first The location of the predicted time; To predict the number of steps.

[0065] 3. Definition of UAV state vector and control input: To avoid redefining symbols, the following formulas will uniformly use the following notation: The predicted drone positions at each time point are The speed is yaw angle is ; For three-dimensional acceleration, Angular velocity represents the yaw angle.

[0066] No. The state vector of the UAV at each predicted time point is defined as: (4) In the formula: For the first The state vector of the UAV at each predicted time point; For the first The three-dimensional position of the drone at a predicted moment; For the first The three-dimensional velocity of the drone at a predicted moment; For the first The yaw angle of the drone at a predicted moment.

[0067] No. The control input of the UAV at each predicted time point is defined as follows: (5) In the formula: For the first The control input for the drone at a predicted time; For the first The three-dimensional acceleration of the drone at a predicted moment; For the first The yaw angle and angular velocity of the drone at the predicted moment.

[0068] The control input sequence in the prediction time domain is defined as: (6) In the formula: To predict the control input sequence in the time domain; For the first Control input at each predicted time; To predict the number of steps.

[0069] 4. Discrete kinematics model of UAV: In a single sampling period Inside, assuming three-dimensional acceleration and yaw angle angular velocity If it remains unchanged, then: (7) In the formula: For the first The predicted location of the drone at a given moment; For the first The predicted location of the drone at a given moment; For the first The speed of the drone at the predicted moment; For the first The acceleration of the drone at a predicted moment; The period is the discrete sampling period.

[0070] (8) In the formula: For the first The speed of the drone at the predicted moment; For the first The speed of the drone at the predicted moment; For the first The acceleration of the drone at a predicted moment; The period is the discrete sampling period.

[0071] (9) In the formula: For the first The yaw angle of the drone at the predicted moment; For the first The yaw angle of the drone at the predicted moment; For the first The yaw angle and angular velocity of the drone at the predicted moment; The period is the discrete sampling period.

[0072] in .

[0073] 5. Smoothing penalty auxiliary function: To avoid discontinuities in the objective function gradient or instability in numerical solutions caused by using piecewise functions, this embodiment defines a smoothing positive part function: (10) In the formula: This is the smoothing positive part function (softplus function); Let be the independent variable of the function, representing the amount of constraint violation; For smoothing parameters ( ), used to control functions in The smoothness of the surrounding area The smaller the value, the closer the function is to the ideal positive part. .

[0074] Further define the smooth squared penalty function: (11) In the formula: For smooth squared penalty function; The smoothed positive part function defined by formula (10); It is the independent variable of the function; For smoothing parameters, this function is used to construct continuously differentiable cost terms corresponding to soft constraints such as safety distance, tracking distance, and field-of-view occlusion.

[0075] when hour, It increases with the degree of constraint violation; when When the value approaches zero, the function can be used to construct continuously differentiable cost terms corresponding to soft constraints such as safety distance, tracking distance, and field-of-view occlusion.

[0076] 6. Objective function: In this invention, the objective function includes target distance maintenance cost, obstacle avoidance safety cost, acceleration change cost, field of view maintenance cost, control input cost, and tracking orientation guidance cost. The stage cost of each prediction step is defined as: (12) In the formula: For the first The stage cost of each prediction step; to These are the non-negative scalar weight coefficients corresponding to the six types of cost terms; Maintaining distance to the target comes at a cost; To avoid the safety costs; The cost of acceleration change; The cost of maintaining the field of view; To control input costs; The cost of tracking and guiding the direction.

[0077] in, to These are the non-negative scalar weight coefficients corresponding to the six types of cost terms, and .

[0078] The total cost function in the prediction time domain is: (13) In the formula: This is the total cost function for prediction in the time domain; To control the input sequence; For the first The stage cost of each prediction step; To predict the number of steps; This is the index for discrete prediction steps.

[0079] Right now (14) In the formula: The total cost function; to These are the weighting coefficients; to There are six types of cost items; For the prediction step index; Let be the number of prediction steps. This formula is the expansion of formula (13).

[0080] The objective of the model predictive control optimization problem is: (15) In the formula: This is the optimal control input sequence obtained in the current control cycle; Represents the independent variable that minimizes the objective function; The total cost function; To control the input sequence.

[0081] 7. Cost of maintaining target distance: To balance stable tracking and target collision avoidance, a reasonable tracking distance needs to be maintained between the UAV and the moving target. The Euclidean distance between the UAV and the target at each predicted time point is defined as: (16) In the formula: For the first The Euclidean distance between the drone and the target at each predicted time; For the first The predicted location of the drone at a given moment; For the first The location of the target at the predicted time; This represents the Euclidean norm.

[0082] set up Let represent the minimum and maximum allowable tracking distances used in the current control cycle, respectively. Then, the target distance maintenance cost is defined as: (17) In the formula: For the first The cost of maintaining the target distance in each prediction step; The smoothing square penalty function is defined for formula (11); Minimum allowable tracking distance; Maximum allowable tracking distance; For the first The predicted Euclidean distance between the drone and the target at each instant. or The time cost increases, when The time cost is zero.

[0083] 8. The cost of obstacle avoidance safety: Let the points on the surface of obstacles or the points occupied by obstacles in a local environment form a set. For any spatial location The Euclidean distance from the nearest obstacle is defined as: (18) In the formula: For any position in space Euclidean distance to the nearest obstacle; For any position in space; It is a set of points on the surface of obstacles or occupied points in a local environment; For set Any obstacle point in the map.

[0084] No. The Euclidean distance from the drone to the nearest obstacle at each predicted time point is: (19) In the formula: For the first The predicted Euclidean distance from the drone to the nearest obstacle at a given moment; This is a distance function to the nearest obstacle; For the first The predicted location of the drone at a given moment.

[0085] set up The obstacle avoidance safety cost is defined as follows, based on the preset safe distance: (20) In the formula: For the first The safety cost of obstacle avoidance in each predicted step; It is a smoothed positive part function; To preset a safe distance ( ); For the first The predicted Euclidean distance from the drone to the nearest obstacle at a given moment. When the time cost is close to zero, The punishment gradually increased over time.

[0086] when At the initial stage, the cost is close to zero; as the drone gradually approaches the nearest obstacle and falls below the preset safe distance, the penalty gradually increases.

[0087] 9. Cost of changes in acceleration: No. The cost of acceleration change in each prediction step is defined as: ;(twenty one) In the formula: For the first The cost of acceleration change in each prediction step; For the first Three-dimensional acceleration for each prediction step; For the first Three-dimensional acceleration for each prediction step; For the prediction step index; This is the number of prediction steps. This cost is used to constrain the rate of change of acceleration between adjacent time steps, ensuring flight smoothness.

[0088] 10. Field of view maintenance constraints: To keep the moving target within the UAV's effective field of view and reduce the risk of obstacles obscuring the target, the field of view maintenance cost takes into account both the target's azimuth error relative to the UAV and the risk of obstacles obscuring the line of sight.

[0089] No. The horizontal line-of-sight azimuth angle of the drone pointing towards the target at the predicted time is: ;(twenty two) In the formula: For the first The horizontal line-of-sight azimuth angle of the UAV pointing towards the target at the predicted moment; atan2 is the four-quadrant arctangent function; , For the first The horizontal position coordinates of the target at each predicted time point; , For the first The predicted horizontal position coordinates of the drone at the specified time.

[0090] Define the yaw angle error as: ;(twenty three) In the formula: For the first Yaw angle error at each predicted time; The azimuth angle of the line of sight; yaw angle of the UAV; sin and cos are sine and cosine functions, respectively; atan2 is the arctangent function in the four quadrants, used to normalize the angle difference to the range [-π, π].

[0091] The target azimuth cost is: ;(twenty four) In the formula: For the first The target azimuth cost of each prediction step; This is the yaw angle error. This cost is used to penalize the deviation between the target's line-of-sight direction and the UAV's yaw direction.

[0092] Furthermore, setting the line of sight between the drone and the target A spherical detection area. The normalized position parameters of each region on the line segment are: (25) In the formula: For the first Normalized position parameters of a spherical detection region on a line segment; Index for spherical detection regions; This represents the total number of spherical detection areas.

[0093] Its center is: (26) In the formula: For the first The predicted time point The location of the center of each spherical detection area; For the first The predicted location of the drone at a given moment; For the first The location of the target at the predicted time; These are the normalized position parameters.

[0094] Let the horizontal and vertical field of view of the camera be respectively and Define the conservative effective half-field of view: (27) In the formula: To conservatively control the effective half-field of view; This refers to the horizontal field of view of the camera; is the camera's vertical field of view; min is the minimum function. Half the smaller of the two values ​​is used to conservatively estimate the effective field of view.

[0095] The radius of the corresponding spherical detection region is defined as: (28) In the formula: For the first The predicted time is... The radius of the spherical detection area; Normalized position parameters; For the target location; Let be the drone's position; tan is the tangent function; To conservatively determine the effective half-field of view.

[0096] No. The Euclidean distance from the center of each ball to the nearest obstacle is: (29) In the formula: For the first The predicted time point The Euclidean distance from the center of a spherical detection area to the nearest obstacle; This is a distance function to the nearest obstacle; This refers to the center position of the spherical detection area.

[0097] when When this occurs, it indicates that an obstacle has entered the corresponding spherical detection area, and there is a risk of obstruction to the target's line of sight. The occlusion cost is defined as: (30) In the formula: For the first The cost of occlusion in each prediction step; It is a smoothed positive part function; For the first The radius of the spherical detection area; For the first The distance from the center of the ball to the nearest obstacle; This represents the total number of spherical detection areas. For region indexing. When This indicates that an obstacle has entered the detection area, posing a risk of obstruction.

[0098] In summary: (31) In the formula: For the first The cost of maintaining the field of view for each prediction step; Cost of the target azimuth angle; The relative weighting coefficient for occlusion cost ( ); The cost of obstruction. This cost takes into account both azimuth deviation and the risk of line-of-sight obstruction.

[0099] 11. Controlling input costs: Definition of the first The control input cost of the step is: (32) In the formula: For the first The control input cost for each prediction step; For the first The control input for the drone at a predicted time; For the prediction step index; To predict the number of steps.

[0100] 12. Tracking direction guidance item: To address the issue of "distance requirements met but relative orientation not ideal" that may arise when only target distance maintenance is used, an ideal following point is defined based on the target's motion direction. The regularized vector for the target's predicted velocity direction is: (33) In the formula: For the first The regularized unit vector of the target's predicted velocity direction at each prediction time; For the first The predicted velocity of the target at each prediction time; ||·| is the vector norm; To prevent small positive numbers from being divided by zero ( ).

[0101] The expected following distance is defined as: (34) In the formula: To determine the desired following distance; Minimum allowable tracking distance; This represents the maximum permissible tracking distance. The expected following distance is the average of the minimum and maximum tracking distances.

[0102] The ideal following point is: (35) In the formula: For the first The ideal following point for a predicted moment; For the first The predicted location of the target at each predicted time; This is the regularized vector in the direction of the target velocity. The ideal following point is located where the target is offset in the opposite direction of the velocity. Place.

[0103] The cost of tracking and directional guidance is: (36) In the formula: For the first The cost of tracking orientation guidance for each prediction step; For the first The predicted location of the drone at a given moment; For the first An ideal following point is predicted at a given moment. This cost is incurred by penalizing the distance between the drone's position and the ideal following point, guiding the drone to approach the optimal observation position directly behind the target.

[0104] When the target speed is lower than the preset effective speed threshold for the direction, the most recent effective target motion direction can be used; if no effective historical motion direction exists, the control cycle can be temporarily suspended. Set it to zero.

[0105] 13. Constraints on the Motion Capabilities of Unmanned Aerial Vehicles: Three-dimensional acceleration satisfies: (37) In the formula: For the first Three-dimensional acceleration for each prediction step; For the maximum allowable acceleration ( ); For the prediction step index; The number of prediction steps. This constraint limits the acceleration amplitude at each time step to no more than the maximum value. The acceleration is a three-dimensional vector, and each component can be positive, negative, or zero.

[0106] Three-dimensional velocity satisfies: (38) In the formula: For the first Three-dimensional velocity at each predicted moment; For the maximum permissible speed ( ); For the prediction step index; The number of prediction steps. This constraint limits the velocity amplitude at each time step to no more than the maximum value. The velocity is a three-dimensional vector, and each component can be positive, negative, or zero.

[0107] Yaw angle and angular velocity satisfy: (39) In the formula: For the first The yaw angle and angular velocity for each prediction step; |·| is the absolute value; For the maximum permissible yaw angle and angular velocity ( ); For the prediction step index; The number of prediction steps. This constraint limits the absolute value of the yaw angle angular velocity to no more than the maximum value, and the yaw angle angular velocity can be positive or negative depending on the direction of rotation.

[0108] 14. Model predictive control optimization problem and rolling execution: Solving for each control cycle: (40) In the formula: To control the input sequence; to These are the weighting coefficients; to There are six types of cost items; This is the initial state.

[0109] And it satisfies the discrete kinematic relations described in equations (7) to (9) and the motion capability constraints described in equations (37) to (39). That is, ; The solution yields: (41) In the formula: To obtain the optimal control input sequence; to This is the optimal control input for each prediction step; For the prediction step index; To predict the number of steps.

[0110] The current control cycle executes only the first control input: (42) In the formula: The actual control input executed in the current control cycle; This refers to the actual time corresponding to the current control cycle; This is the first control input in the optimal control input sequence. Model predictive control only executes the first control input in the sequence.

[0111] The next control cycle reacquires the latest state and re-optimizes, thus forming rolling time-domain control.

[0112] 15. Speed-driven tracking distance and adaptive adjustment of distance weight: The current speed of the moving target is: (43) In the formula: The current speed of the moving target; The velocity of the moving target at the current moment.

[0113] Define the truncation function: (44) In the formula: This is a truncation function; The independent variable to be truncated; The lower bound; The upper bound is defined by `max`; `max` is the function to find the maximum value; `min` is the function to find the minimum value. This function will... Limited to [ , Within the range.

[0114] Define the dimensionless velocity factor: (45) In the formula: It is a dimensionless velocity factor; The current speed of the target; This is the speed threshold corresponding to the lower limit of the speed factor; is the speed threshold corresponding to the upper limit of the speed factor; clip is the truncation function. The value range is [0, 1], and it is used to dynamically adjust the tracking parameters according to the target speed.

[0115] in The dynamic tracking distance and distance weights are as follows: (46) In the formula: The minimum dynamic tracking distance; This is the minimum safe tracking distance. Minimum tracking distance as the baseline; is a dimensionless velocity factor; max is the function that maximizes the value. The minimum tracking distance decreases linearly as the target velocity increases.

[0116] (47) In the formula: This represents the dynamic maximum tracking distance. This is the minimum maximum tracking distance for safety. The maximum tracking distance is the baseline. is a dimensionless velocity factor; max is the function that maximizes the value. As the target velocity increases, the maximum tracking distance decreases linearly.

[0117] (48) In the formula: Dynamic weights are used to maintain the cost of the target distance; As the benchmark weight; This is a dimensionless velocity factor. As the target velocity increases, the distance-keeping cost weight increases linearly to ensure close following.

[0118] Parameters satisfy , And ensure based on actual settings .

[0119] In one embodiment, it is preferable , , , , , .

[0120] 16. Adaptive adjustment of prediction steps driven by Euclidean distance to the nearest obstacle: Let the current actual position of the drone be... The Euclidean distance from the current position to the nearest obstacle is: (49) In the formula: The distance from the drone's current actual position to the nearest obstacle is expressed in Euclidean form. This is a distance function to the nearest obstacle; This is the current actual location of the drone.

[0121] set up To determine the congestion distance, a normalized congestion factor is defined: (50) In the formula: This is the normalized congestion factor; Distance for determining crowding ( ); The distance from the current position to the nearest obstacle; clip is the truncation function. The value range is [0, 1]. hour This indicates a crowded environment.

[0122] set up As the baseline prediction step number, The maximum allowed number of prediction steps, and The current control cycle prediction steps are: (51) In the formula: This represents the number of prediction steps for the current control cycle. The baseline prediction steps; This represents the maximum allowed number of prediction steps. `round` is the normalized congestion factor; `round` is the rounding function. When the environment is congested, the number of prediction steps increases linearly between the baseline and the maximum value.

[0123] The corresponding prediction time length is: (52) In the formula: To predict the length of time; To predict the number of steps; The period is the discrete sampling period.

[0124] In one embodiment, it is preferable , , .when hour, The baseline prediction steps are used; as The number of prediction steps gradually increases as the distance decreases. This embodiment adjusts directly based on the current Euclidean distance, without introducing additional environmental state variables.

[0125] 17. Solver hot start and handling of prediction step changes: Let the number of prediction steps in the previous control cycle be... The optimal control input sequence for the previous control cycle was: (53) In the formula: This is the optimal control input sequence for the previous control cycle; to This is the optimal control input for each prediction step in the previous cycle; This represents the number of prediction steps in the previous control cycle.

[0126] when At that time, remove the first control input and shift forward, then fill in the gaps using the control input from the previous prediction time domain end: (54) In the formula: These are the initial guesses for the hot start of the solver in the current control cycle; to This is the portion of the optimal control input sequence shifted from the previous cycle; the last part... Used to fill in the end of the prediction time domain. This formula is applicable to... The situation.

[0127] when At that time, the effective control input at the end of the previous cycle is used to supplement the new prediction step; when When the control input exceeds the current prediction time domain length, it is truncated. The warm-start object always contains the complete control input vector. .

[0128] 18. Reacquisition of the target after a brief loss: When a moving target temporarily leaves the effective sensing range of the drone, record the last effective detection time before the target is lost. Corresponding position and velocity. Predict the target's possible position using a constant velocity model within a preset short time range: (55) In the formula: After the target is lost Predicted location at any given time; The last valid detection time before the target was lost; The location of the target at the moment it was lost; The velocity at the moment the target is lost; The time increment after the data is lost; This represents the maximum prediction time threshold. This formula predicts the possible location of the target based on a constant velocity model.

[0129] After the target is lost, the UAV, under the conditions of satisfying its own motion capability constraints and obstacle safety requirements, approaches the area where the target's last known position and short-term predicted position are located, while adjusting the yaw angle to align the camera's effective field of view with the predicted area. If the target is detected again, normal model prediction control tracking is resumed using the newly obtained target position and velocity; if the target is still not detected, a limited-distance search is performed along the target's last known motion direction, and the yaw angle is periodically adjusted to expand the search field of view; if the target is still not detected after exceeding a preset search time threshold, the current target tracking task is deemed a failure. The reacquisition process only utilizes the motion state and short-term kinematic prediction results obtained before the target was lost.

[0130] 19. Execution sequence of a single control cycle: Each control cycle first acquires the UAV's current 3D position, 3D velocity, and yaw angle, acquires the moving target's current 3D position and 3D velocity, and acquires the spatial position information of local obstacles. Then, it calculates the Euclidean distance from the UAV's current position to the nearest obstacle. The number of prediction steps for the current control cycle is determined according to equations (49) to (52). Update the minimum tracking distance according to the target's current speed using equations (43) to (48). Maximum tracking distance Distance-based cost weighting ; and then based on the current Predict the future trajectory of the target and build a model for predictive control optimization.

[0131] The optimal control input sequence is obtained using the sequential quadratic programming method. Then, only the first control input is executed. The next control cycle will reacquire the latest UAV status, target status, and obstacle spatial information, and re-record the new actual time. Repeat the above process.

[0132] Verification and Analysis: Simulation Environment: The UAV trajectory planning method for target tracking proposed in this invention was implemented using a simple MATLAB environment on a Windows 11 system with an AMD Ryzen 7 7840H processor with Radeon 780M Graphics and 16GB of RAM. Two virtual UAVs were simulated: one as the target UAV and the other as the tracking UAV. The maximum speed of the target UAV was 3 m / s, and the other was 5 m / s. The expected tracking distance was assumed to be between 1 m and 2.5 m. The simulation experiments ultimately analyzed the following five indicators: 1. Tracking orientation guidance ablation analysis to compare the field of view maintenance near the target's turn when adding and removing tracking orientation guidance costs.

[0133] 2. Tracking distance: Real-time changes in the distance between the drone and the target.

[0134] 3. Changes in the target's angle within the field of view, and changes in the angular relationship between the target and the UAV.

[0135] 4. Euclidean distance to the nearest obstacle: Analyze the relationship between the Euclidean distance from the tracking drone to the nearest obstacle and the safe distance threshold.

[0136] 5. Smoothness analysis: Analyze the flight motion of the UAV based on the changes in acceleration.

[0137] Simulation scene design: The simulation area measures 40m × 40m × 8m. Multiple obstacles are set within the scene, and various different motion trajectories are designed for the moving target for comparative analysis. The corresponding tracking simulation visualization results are as follows: Figures 5 to 8 As shown.

[0138] Results analysis: Figures 5-8 Simulation results of UAV target tracking under different target motion trajectories in Experiments 1-4 are shown, along with the environmental conditions of the simulation (see Table 1 for comparison). Experiments 1-4 used the same UAV kinematic model, trajectory planning method, objective function form, control constraints, solution method, obstacle distribution, and obstacle safety boundary settings. The simulation time for each group of experiments was 20 seconds. The main goal was to evaluate the adaptability of the trajectory planning method to different target motion patterns by changing the initial positions of the target and the tracking UAV, as well as the trajectory of the moving target, causing the tracking UAV to experience different turns, obstacle avoidance, and relative position changes. Figure 9 for Figure 5 The tracking performance statistics for the simulation environment shown in Experiment 1 are as follows: Figure 10 for Figure 6 The tracking performance statistics for the simulation environment shown in Experiment 2 are as follows: Figure 11 for Figure 7 The tracking performance statistics for the simulation environment shown in Experiment 3 are as follows: Figure 12 for Figure 8 The tracking performance statistics for the simulation environment shown in Experiment 4 are as follows. Figures 9-12 The target tracking performance under various experimental environments was evaluated by considering the tracking distance between the UAV and the target, the UAV's three-dimensional acceleration, the distance between the UAV and the nearest obstacle, and the changes in the UAV's actual yaw angle and the expected yaw angle.

[0139] Table 1. Comparison of Simulation Condition Settings

[0140] like Figures 9 to 12 As shown, in the four sets of simulation experiments, the actual distance between the UAV and the target generally remained near the set dynamic tracking distance range; the error between the target's line of sight and the UAV's yaw direction remained small; the Euclidean distance between the tracking UAV and the nearest obstacle did not show a sustained situation of being significantly lower than the preset safe distance; the acceleration curve was generally continuous and smooth, indicating that the control input cost and acceleration change cost can jointly suppress excessive control input and drastic changes between adjacent time steps.

[0141] like Figure 13 and Figure 14As shown, under the condition that the map environment and the trajectory of the moving target remain consistent, after adding the tracking orientation guidance cost, the UAV can more proactively adjust to a favorable observation area behind the target's direction of movement when the target approaches an obstacle and makes a turn, thereby reducing the risk of obstructed vision; after removing the guidance cost, the visibility retention performance near the target's turn in some scenarios decreases.

[0142] To quantify the effect of tracking orientation guidance cost, multiple ablation experiments were conducted and the field-of-view stabilization time ratio was statistically analyzed. As shown in Table 2, the field-of-view stabilization time ratio was 100% for all four experimental groups when the tracking orientation guidance cost was included; after removing the cost, the ratios for two groups decreased to 95.5% and 96.5%, respectively, while the remaining two groups maintained 100%. These results indicate that the tracking orientation guidance cost can synergistically improve the observation geometry and tracking continuity in complex obstacle scenarios in conjunction with the field-of-view maintenance cost.

[0143] Table 2. Comparison of Field of View Stability

[0144] In summary, this invention proposes a UAV trajectory planning method for tracking moving targets in complex obstacle environments. This method uses a complete control input sequence consisting of three-dimensional acceleration and yaw angle / angular velocity as optimization variables. It recursively derives the predicted UAV state using a discrete kinematics model and constructs a finite-time optimization problem by integrating the costs of control input, Euclidean distance to the nearest obstacle, target tracking distance, target field of view maintenance, acceleration changes, and tracking orientation guidance. Simultaneously, it adjusts the tracking distance and its weight based on the target velocity and the number of prediction steps based on the Euclidean distance from the UAV's current position to the nearest obstacle. Simulation results show that this method can achieve a smoother tracking trajectory and reduce the risk of target loss due to obstacle occlusion under various map and target trajectory conditions.

[0145] Technical Analysis: The MPC controller described in this invention uses Sequential Quadratic Programming (SQP) to solve the finite-time optimal control problem. This can be implemented using the `fmincon` function in the MATLAB optimization toolbox with the SQP algorithm selected. The optimization decision variable is a complete control input sequence whose length varies with the number of prediction steps N. Each control input includes three-dimensional acceleration and yaw angle / angular velocity. The three-dimensional acceleration magnitude, the velocity magnitude obtained from kinematic recursion, and the acceleration changes between adjacent time steps can be treated as nonlinear inequality constraints. The absolute value limit of the yaw angle / angular velocity can be equivalently set as upper and lower bounds. If the actual program uses separate amplitude limits for each coordinate axis, the corresponding acceleration components can also have their variable boundaries directly set. The objective function includes nonlinear cost terms such as the Euclidean distance to the nearest obstacle, the target distance, field-of-view maintenance, and tracking azimuth. In each iteration, SQP constructs a local quadratic programming subproblem based on the objective function and constraint information of the current point and updates the control input sequence.

[0146] The solver can approximate the second-order information of the objective function using a quasi-Newton approach, and determine convergence or termination based on conditions such as first-order optimality, step size, constraint violation degree, and maximum number of iterations. Specific tolerance parameters are set according to the control cycle and computing platform to balance solution accuracy and online computational burden.

[0147] To improve the efficiency of the rolling time-domain solution, the current control cycle uses the optimal control input sequence from the previous control cycle for a warm start, and the sequence is padded or truncated based on changes in the number of prediction steps. This processing directly applies to the complete control input sequence, which includes three-dimensional acceleration and yaw angle angular velocity.

[0148] For motion capability constraints such as speed, acceleration, yaw angle, angular velocity, and acceleration changes, the feasibility of candidate control sequences is limited by constraints during the solution process. For requirements such as obstacle safety, target distance, and field of view occlusion, a smoothing penalty cost is reflected in the objective function, thereby guiding the trajectory away from high-risk areas while maintaining the solvability of the optimization problem.

[0149] In terms of real-time performance, by reasonably limiting the maximum number of iterations and convergence tolerance, using the solution from the previous cycle for hot start, and adaptively adjusting N between the baseline prediction steps and the maximum prediction steps based on the environmental space margin, the optimization dimensionality can be reduced in open environments and the planning foresight can be increased when obstacles are close.

Claims

1. A UAV trajectory planning method for target tracking, characterized in that, include: Step S1: Real-time acquisition of the current status information of the UAV, the current status information of the moving target, and the Euclidean symbolic distance field of the surrounding environment; The current status information of the UAV includes its three-dimensional position, three-dimensional velocity, and yaw angle; the current status information of the moving target includes its three-dimensional position and three-dimensional velocity. Step S2: Based on the current state information of the moving target, a constant velocity model is used to predict the target's predicted trajectory for a preset number of N time steps in the future, with each time step corresponding to the target's predicted position at a prediction time. Step S3: Construct a trajectory planning model based on model predictive control. Use the current state information of the UAV as the initial state and the UAV acceleration sequence over the next N time steps as optimization variables. Set the following constraints: kinematic constraints recursively derived from the current state information of the UAV; obstacle avoidance safety constraints based on the Euclidean sign distance field; target distance holding constraints to maintain the distance between the UAV and the target; field-of-view holding constraints to keep the target within the UAV's field of view; acceleration smoothing constraints to constrain the rate of change of acceleration; and tracking orientation guidance constraints to guide the UAV to approach the target's velocity direction directly behind it. The field-of-view holding constraints evaluate the occlusion on the line connecting the UAV and the target based on the yaw angle, a preset square pyramid field-of-view model, and a spherical region fitting method. The tracking orientation guidance constraints define an ideal following point, located at a fixed distance offset from the target's predicted position along the opposite direction of the target velocity vector on the predicted target trajectory, and constrain the UAV's position to approach the ideal following point. Step S4: Construct an objective function that includes the kinematic constraints, obstacle avoidance safety constraints, target distance maintenance constraints, field of view maintenance constraints, acceleration smoothing constraints, and tracking orientation guidance constraints, and use a sequential quadratic programming solver to solve the optimization variables in the rolling time domain to obtain the optimal acceleration sequence; Step S5: Generate a sequence of trajectory points for the UAV over the next N time steps based on the optimal acceleration sequence and the current state information of the UAV, and execute the control command for only the first time step. Step S6: In the next control cycle, return to step S1 and repeat steps S1 to S5.

2. The UAV trajectory planning method for target tracking according to claim 1, characterized in that, The objective function consists of multiple weighted terms, including: a first weighted term characterizing acceleration smoothness, a second weighted term characterizing the safe distance between the UAV and the obstacle, a third weighted term characterizing the deviation between the actual distance and the expected distance between the UAV and the target, a fourth weighted term characterizing the angular deviation of the target in the UAV's field of view, a fifth weighted term characterizing the rate of change of acceleration in adjacent time steps, and a sixth weighted term characterizing the deviation between the UAV's position and the ideal following point; each weighted term is multiplied by a preset weight coefficient and then summed to form the objective function.

3. The UAV trajectory planning method for target tracking according to claim 1, characterized in that, The method for constructing the field-of-view constraint is as follows: construct the regular square pyramid field-of-view model with the UAV as the vertex; set multiple spherical regions on the line connecting the UAV and the target, the center position and radius of each spherical region are determined by the three-dimensional position, the preset field-of-view angle and the preset interval parameter; obtain the signed distance value from the center of each spherical region to the nearest obstacle by querying the Euclidean signed distance field; when the signed distance value is less than the radius of the corresponding spherical region, it is determined that there is occlusion at the location of the spherical region, and a differentiable cost function is constructed accordingly as the field-of-view constraint.

4. The UAV trajectory planning method for target tracking according to claim 1, characterized in that, The fixed distance in the tracking orientation guidance constraint is the average of the preset minimum tracking distance and the preset maximum tracking distance; the ideal following point is located at the target predicted position offset by the fixed distance along the opposite direction of the target velocity vector. The tracking orientation guidance constraint is implemented by penalizing the Euclidean distance between the actual position of the UAV and the ideal following point.

5. The UAV trajectory planning method for target tracking according to claim 2, characterized in that, It also includes a parameter adaptive adjustment step: dynamically adjusting the minimum tracking distance, maximum tracking distance, and the weight of the third weighting term in the target distance maintenance constraint according to the real-time speed of the moving target; and dynamically adjusting the prediction step number N according to the Euclidean symbolic distance field value of the current position of the UAV.

6. The UAV trajectory planning method for target tracking according to claim 5, characterized in that, The specific steps of dynamically adjusting the minimum tracking distance, maximum tracking distance, and the weight of the third weighting term based on the real-time speed of the moving target include: calculating the speed of the moving target and defining a dimensionless speed factor; when the speed of the moving target increases, linearly reducing the minimum tracking distance and the maximum tracking distance, and linearly increasing the weight of the third weighting term; when the speed of the moving target decreases, linearly restoring the weights of the minimum tracking distance, the maximum tracking distance, and the third weighting term to their respective baseline values.

7. The UAV trajectory planning method for target tracking according to claim 5, characterized in that, The specific steps of dynamically adjusting the prediction step number N based on the Euclidean symbolic distance field value of the current position of the UAV include: obtaining the Euclidean symbolic distance field value of the current position of the UAV; calculating the congestion factor based on the Euclidean symbolic distance field value and a preset congestion determination distance; when the Euclidean symbolic distance field value is less than the congestion determination distance, linearly increasing the prediction step number N according to the congestion factor, and the value of N after the increase does not exceed the preset maximum prediction step number; and performing a first-order low-pass filter smoothing process on the Euclidean symbolic distance field value to avoid frequent jumps in the prediction step number.

8. The UAV trajectory planning method for target tracking according to claim 1, characterized in that, It also includes a target re-acquisition step: when a moving target is lost from the UAV's field of view, the UAV first flies to the target's last known location at the maximum safe speed. The last known location of the target is the three-dimensional position of the moving target last acquired before it was lost. After reaching the last known position of the target, the UAV adjusts its yaw angle to point to the short-term estimated position calculated based on the last known speed and direction of movement of the target, and flies forward in the direction pointed to by the yaw angle to perform detection; if the target is still not detected, the UAV switches to flying along the last known direction of movement of the target, and periodically performs yaw maneuvers during the flight to expand the search field of view, while using particle filtering to generate multiple candidate target positions to guide the search; If the target cannot be recaptured within the preset time threshold, the tracking is deemed to have failed.

9. The UAV trajectory planning method for target tracking according to claim 1, characterized in that, The kinematic constraints of the UAV include: the acceleration amplitude at each time step does not exceed the preset maximum acceleration, the velocity amplitude at each time step does not exceed the preset maximum velocity, the yaw angle angular velocity amplitude at each time step does not exceed the preset maximum yaw angle angular velocity, and the rate of change of acceleration between adjacent time steps does not exceed the preset maximum rate of change of acceleration.

10. The UAV trajectory planning method for target tracking according to claim 1, characterized in that, In step S4, when using a sequential quadratic programming solver to solve for the optimization variables, a hot start method is adopted: the acceleration of the first time step in the optimal acceleration sequence obtained from the previous control cycle is removed, and the remaining acceleration sequence is shifted forward by one time step in sequence. The shifted sequence is then used as the initial guess value of the sequential quadratic programming solver for the current control cycle.

Images