Method for planning trajectory of unmanned aerial vehicle, related apparatus and application

HK30137248AActive Publication Date: 2026-09-04THE HONG KONG POLYTECHNIC UNIV
View PDF 0 Cites 0 Cited by

Patent Information

Application Number
HK32026125422
Authority / Receiving Office
HK · HK
Patent Type
Applications
Current Assignee / Owner
Priority Date
2025-11-19
Filing Date
2026-06-29
Publication Date
2026-09-04
Estimated Expiration
2034-06-28

Smart Images

  • Figure 00000020_0000
    Figure 00000020_0000
  • Figure 00000020_0001
    Figure 00000020_0001
  • Figure 00000021_0000
    Figure 00000021_0000
Patent Text Reader

Abstract

This application discloses a method, related apparatus, and application for UAV trajectory planning. The method first establishes an optimal control problem model under free terminal time. By introducing a time mapping operator, the original time domain is mapped to a unified normalized interval, thereby treating time variables and state variables as optimization objects. On this basis, the model is discretized. By iteratively solving the discrete nodes, the joint optimal allocation of time and trajectory can be achieved. This method can effectively reduce computational complexity while ensuring accuracy and can be applied to task scenarios such as path optimization, energy allocation, and dynamic obstacle avoidance in multi-UAV cooperative systems.
Need to check novelty before this filing date? Find Prior Art

Description

Specification 1 A Method, Related Devices, and Applications for UAV Trajectory Planning Technical Field This invention belongs to the field of UAV trajectory planning technology, and particularly relates to a method, related devices, and applications for UAV trajectory planning. Background Technology UAVs are increasingly widely used in logistics transportation, environmental monitoring, disaster search and rescue, and inspection and security. The efficiency and safety of their missions largely depend on the quality of the trajectory planning method. Trajectory planning refers to generating a feasible flight trajectory for a UAV that satisfies dynamic constraints, environmental constraints, and mission requirements under known environmental constraints and mission objectives. This trajectory should typically achieve a comprehensive balance among indicators such as flight time, energy consumption, path smoothness, and safety distance. Existing UAV trajectory planning methods can be broadly classified into search-based discrete planning methods and optimization-based continuous planning methods. Search-based methods (such as A* algorithm, Dijkstra's algorithm, RRT and its improved algorithms) have the advantages of simple implementation and strong global feasibility. However, since they essentially perform path search in discrete space, it is difficult to fully consider the dynamic constraints and control input limitations of the UAV. The generated paths often require subsequent smoothing or re-optimization, and the computational complexity increases sharply in high-dimensional continuous space. In contrast, optimization-based trajectory planning methods (such as nonlinear programming and model predictive control) can directly solve trajectories in continuous space, simultaneously considering dynamic constraints such as position, velocity, and acceleration, and balancing safety and energy efficiency during the optimization process. However, these methods typically require solving nonconvex optimization problems, are prone to getting trapped in local optima, are sensitive to initial solutions, and have a heavy computational burden, making them unsuitable for real-time planning or complex task scenarios. In recent years, the academic and engineering communities have proposed various improvement approaches to address the nonconvexity problem in UAV trajectory planning. For example, by using problem decomposition, augmented Lagrangian methods, convex relaxation, and iterative linearization, nonconvex optimization can be transformed into a series of solvable subproblems. These methods have improved the convergence and feasibility of the solution to some extent, but still have the following shortcomings: On the one hand, the solution process of nonconvex problems under complex constraints still relies heavily on heuristics or manual adjustments; on the other hand, existing methods often struggle to simultaneously ensure the dynamic feasibility of the trajectory while considering multiple optimization objectives such as control energy consumption, task time, and system constraints. Furthermore, in multi-obstacle, unstructured environments, achieving fast solution and accurate constraint satisfaction while ensuring algorithm convergence remains a challenge for current research. In summary, existing UAV trajectory planning methods still have limitations in handling non-convex constraints, optimizing solution efficiency, and coordinating multiple objectives, making it difficult to simultaneously consider safety, real-time performance, and resource utilization efficiency. Therefore, it is still necessary to provide a new UAV trajectory planning method to improve planning efficiency and optimize control resource allocation while ensuring trajectory feasibility.Based on this, the present invention aims to propose a UAV trajectory planning method, related devices, and applications. By using time mapping to convert the terminal time of a continuous-time optimization problem from a free terminal time to a fixed terminal time, a trajectory planning model for the UAV is established, and a sequential convex programming algorithm is used to solve for the flight trajectory with optimal flight time. In a first aspect, the present invention provides a UAV trajectory planning method, comprising: constructing constraints based on the UAV's dynamic equations; establishing a first trajectory planning model with the goal of minimizing the control quantity; the optimization solution interval of the first trajectory planning model is the free terminal time; using a time mapping operator to convert the optimization solution interval of the first trajectory planning model from the free terminal time to the fixed terminal time, obtaining a second trajectory planning model; discretizing the second trajectory planning model to obtain a discretized trajectory planning model; taking a linear time-invariant system with a fixed terminal time as the object, establishing an initial trajectory value model for the UAV, and solving the initial trajectory value model to obtain initial trajectory values; using the initial trajectory values, using a sequential convex programming algorithm to solve the discretized trajectory planning model to obtain the optimal flight trajectory of the UAV. Furthermore, the constraints of the second trajectory planning model include dynamic constraints. Discretizing the second trajectory planning model to obtain a discretized trajectory planning model includes: dividing the second trajectory planning model into several sub-intervals; for each sub-interval, using the first-order hold interpolation method to discretize the control variables in the dynamic constraints to obtain discretized control variables; and based on the discretized control variable representation, converting the dynamic constraints in the constraints into state transition constraints to complete the discretization of the second trajectory planning model, thus obtaining the discretized trajectory planning model. Furthermore, using the time mapping operator to convert the optimization solution interval of the first trajectory planning model from free terminal time to fixed terminal time includes: defining the optimization solution interval of the first trajectory planning model as [0, ft t ], where 0t represents the initial time and ft represents the terminal time, and at least one of 0 , ft t is an optimization variable; mapping the optimization solution interval [0, ft t] under free terminal time to the optimization solution interval [0, f  ] under fixed terminal time according to the following mapping relationship: 0 0 ( ) ( ) , ( ) , ( )ff dt sttttd        .Furthermore, by using a time mapping operator to transform the optimization solution interval of the first trajectory planning model from free terminal time to fixed terminal time, the second trajectory planning model is obtained by: Based on the optimization solution interval with fixed terminal time, using the time mapping operator and control variables, the system state variables of the dynamic equation are extended to obtain extended state variables. These extended state variables simultaneously represent the system state variables, system control variables, and the optimization solution interval. Based on the extended state variables and the time mapping operator, the first trajectory planning model is transformed into the second trajectory planning model. Furthermore, before solving the discretized trajectory planning model, the above method also includes: introducing a virtual control term into the state transition equation of the constraints of the discretized trajectory planning model, and introducing a penalty term corresponding to the virtual control term into the objective function, making the solution of the discretized trajectory planning model feasible in the linearized domain. Furthermore, the constraints include dynamic constraints, landing point constraints, and control variable boundary constraints. Furthermore, the initial trajectory values ​​are obtained by solving the initial trajectory value model, which includes: defining the optimization objective of the initial trajectory value model as minimizing acceleration, and the constraints include system state transition constraints, flight corridor trajectory point constraints, and landing point constraints; and then solving the initial trajectory value model to obtain the initial trajectory values. Secondly, this invention provides a UAV trajectory planning device, comprising: a first modeling module, used to construct constraints (HK 30137248 A 4) for the trajectory planning problem based on the dynamic equations of the UAV, and to establish a first trajectory planning model with the goal of minimizing the control quantity, wherein the optimization solution interval of the first trajectory planning model is the free terminal time; a second modeling module, used to convert the optimization solution interval from the free terminal time to a fixed terminal time using a time mapping operator, to obtain a second trajectory planning model; a model discretization module, used to discretize the second trajectory planning model to obtain a discretized trajectory planning model; a trajectory initial value solution module, used to establish a trajectory initial value model for the UAV as a linear time-invariant system with a fixed terminal time, and to solve the trajectory initial value model to obtain the trajectory initial value; and an optimal trajectory solution module, used to solve the discretized trajectory planning model using a sequential convex programming algorithm based on the trajectory initial value to obtain the optimal flight trajectory of the UAV. Thirdly, this invention provides an electronic device, comprising a memory storing computer-executable instructions and a processor, wherein when the computer-executable instructions are executed by the processor, the device performs the various steps of the UAV trajectory planning method provided in the first aspect. Fourthly, the present invention provides a readable storage medium storing a computer-executable program, which, when executed, implements the various steps of the UAV trajectory planning method provided in the first aspect. Embodiments of the present invention also provide an application of the method provided in the first aspect in a UAV cooperative system.Compared with existing trajectory planning schemes, this invention has the following beneficial effects: This invention proposes a UAV trajectory planning method, related devices, and applications. The method establishes a planning problem under free terminal time, ensuring that the duration allocation of each solution sub-interval can be adjusted according to the flight path during the solution process, thereby adaptively allocating flight corridors along the flight trajectory. By introducing a time mapping operator, the original trajectory planning problem under free terminal time is transformed into a fixed terminal time problem, making the optimization interval fixed, thus avoiding the computational complexity and numerical instability caused by using the terminal time as an optimization variable. A further embodiment extends the system state variables, allowing the extended state variables to simultaneously represent the system's state information, control quantities, and time mapping relationships, realizing unified modeling of the free terminal time problem to the fixed terminal time problem, providing a complete mathematical description for subsequent discretization and optimization solutions. A further embodiment introduces virtual control terms and corresponding penalty terms into the discretized planning model, ensuring that the linearized trajectory planning model maintains a feasible solution during the iteration process, enhancing the model's convergence stability. This invention can meet constraints including UAV dynamics constraints. Under these conditions, an optimal flight trajectory that is dynamically safe, feasible, computationally stable, and highly efficient is obtained. Brief Description of the Drawings To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are merely embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on the provided drawings without creative effort. Figure 1 is a flowchart of the UAV trajectory planning method provided in the embodiment of the present invention; Figure 2 is a structural schematic diagram of the UAV trajectory planning device provided in the embodiment of the present invention; Figure 3 is an electronic device architecture diagram provided in the embodiment of the present invention. Detailed Description of the Embodiments The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are merely some embodiments of the present invention, not all embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative effort are within the scope of protection of the present invention. Referring to Figure 1, an embodiment of the present invention provides a UAV trajectory planning method, including the following steps: Step S110. Construct constraints based on the dynamic equations of the UAV, and establish a first trajectory planning model with the goal of minimizing the control quantity. The optimization solution interval of the first trajectory planning model is the free terminal time. In this step, the first step is to establish the dynamic equations of the UAV to describe the motion of the UAV during flight.Specifically, the dynamic equations of a UAV can be described using both state variables and control variables. State variables include the UAV's position, velocity, attitude angles, etc., while control variables typically include thrust, roll rate, pitch rate, and yaw rate. The relationship between state and control variables is described by a nonlinear function of the system dynamics. Based on the established dynamic equations, constraints for the trajectory planning problem can be constructed, including dynamic constraints, control variable boundary constraints, position or corridor constraints, and terminal constraints. On this basis, with the minimization of control variables as the optimization objective, a first trajectory planning model under free terminal time is established. In optimal control theory (OCP), the trajectory planning problem typically includes state equations, control constraints, and a time interval. When the terminal time is fixed, the system optimizes the control variables only within a given time period. However, the free terminal time problem allows the optimization interval to be solved as an optimization variable along with the control and state variables. Therefore, solving the first trajectory planning model not only yields the optimal form of the control law but also the optimal flight time. Establishing a free terminal time model in the continuous time domain can fully describe the two-level optimization problem of "how the UAV flies + how long it flies optimally," which is more in line with the energy constraints and mission execution characteristics of the UAV. In a further embodiment, to ensure that the model can be efficiently solved by the Sequential Convex Programming (SCP) algorithm, functional constraints and variable selection are used during the modeling stage to ensure that both the objective function and constraints are convex. Specifically, the objective function can be represented by a strictly convex function in mathematical expression, ensuring that the control input at each moment has a unique optimal solution; the constraints are expressed through linearization or affine forms, linear inequalities, etc., ensuring the convexity and closure of the constraint space. Through functional expression design, the first trajectory planning model is constructed as a convex optimal control problem defined in continuous time, whose mathematical form is easy to discretize and process by convex optimization algorithms. Step S120: Using the time mapping operator, the optimization solution interval of the first trajectory planning model is converted from free terminal time to fixed terminal time to obtain the second trajectory planning model. Since solving the free terminal time problem is complex, this step introduces a time mapping operator to facilitate subsequent solutions using standard optimal control or sequential convex programming methods. This operator transforms the original optimization solution interval from variable free terminal time to fixed terminal time, achieving time domain normalization and reducing computational complexity. Ultimately, a second trajectory planning model with a fixed terminal time optimization solution interval is obtained.In a further embodiment, the optimization solution interval of the first trajectory planning model is defined as [0, ft t ], where 0t represents the initial time and ft represents the terminal time. At least one of 0 and ft t is an optimization variable. A time mapping function ( )t  is defined and is made to satisfy the boundary condition 0 0( ) , ( )f ft ttt   . At the same time, the time scale is defined as: ( ) ( ) dt sd   . Through this mapping operator, the optimization solution interval [0, ft t ] under the free terminal time is mapped to the optimization solution interval [0, f  ] under the fixed terminal time, which is used to characterize the ratio of physical time to normalized time. Furthermore, for the continuous-time model of the system at a certain free terminal time, extended state variables are established based on the time mapping to form an augmented system model. This eliminates steady-state errors caused by model linearization approximation, external disturbances, and static deviations, and provides a unified state structure for subsequent discretization and predictive control. Specifically, the system state variables of the dynamic equation are extended using the time mapping operator and control variables to obtain extended state variables. These extended state variables simultaneously represent the system state variables, system control variables, and the optimization solution interval. Based on the extended state variables and the time mapping operator, the first trajectory planning model is transformed into a second trajectory planning model. Step S130: Discretize the second trajectory planning model to obtain a discretized trajectory planning model. This step discretizes the time-mapped second trajectory planning model to obtain a discretized trajectory planning model that can be solved at a fixed time step. The core of this step is to transform the continuous-time optimization problem into a finite-dimensional discrete optimization problem through the discretization of time and control variables, thereby enabling efficient trajectory planning calculations using numerical solution algorithms. Specifically, linearizing the nonlinear dynamics at a reference point or trajectory allows the dynamic constraints to be written in affine form, ensuring the convexity of the constraint set and facilitating the use of convex optimization methods (such as sequential convex programming). Subsequently, this linearized expression is discretized (using methods such as zero-order preservation, matrix exponentiation, or numerical integration) to transform continuous-time constraints into finite-dimensional state transition constraints, facilitating numerical solution and iterative updates. This constitutes a solvable, stable, and efficient numerical solution framework. In a further embodiment, considering that slight inconsistencies may arise between the system's state transition equations and control constraints after adopting a linearized model and discretization, especially when the linearized point deviates from the actual flight trajectory or external disturbances exist, direct solution may lead to an infeasible optimization problem. Therefore, this embodiment introduces a virtual control term into the constraints of the discretized model to compensate for state deviations caused by linearization errors and numerical discretization errors.The virtual control term is an adjustable relaxation variable added to the state transition constraints, allowing for a certain range of state deviations during the optimization process, thus ensuring the model remains feasible within the local linearization domain. To prevent the introduction of the virtual control term from undermining the physical meaning of the optimization objective, this embodiment simultaneously introduces a penalty term corresponding to the virtual control term into the objective function, making it as close to zero as possible numerically. When the system satisfies the linearization assumption under ideal conditions, the virtual control term automatically converges to zero; when nonlinear disturbances or modeling errors exist, the virtual control term corrects the state transition equation with minimal magnitude, ensuring that the overall trajectory optimization can still converge to a stable solution. During execution, the system first calculates the state transition relationship at each discrete time step based on the discretized time step, and then introduces the corresponding virtual control component at each time step. In the optimization solution stage, the algorithm simultaneously minimizes the weighted sum of the control quantity and the virtual control quantity, ensuring the smoothness of the control input and the feasibility of the state equation. Through this mechanism, the discretized trajectory planning model can obtain a globally feasible optimization path while ensuring numerical stability, effectively avoiding solution failure or local non-convergence caused by linearization. By introducing a virtual control term, the system not only improves the robustness and solution stability of the discretized trajectory model, but also enhances the adaptability of the algorithm under complex environments or strongly nonlinear dynamic conditions, thus providing a reliable feasible initial value and optimization basis for subsequent sequential convex programming solutions. Step S140. Taking a linear time-invariant system with a fixed terminal time as the object, an initial trajectory value model is established for the UAV, and the initial trajectory value is obtained by solving the initial trajectory value model. This step takes a linear time-invariant system with a fixed terminal time as the object, establishes an initial trajectory value model for the UAV, and optimizes the model to obtain the initial trajectory value for subsequent sequential convex programming solutions. This achieves the goal of obtaining an initial feasible trajectory that satisfies the basic constraints without directly solving complex nonlinear models, thus providing a convergence basis and search direction for subsequent convexization iterations. Specifically, the optimization objective of the initial trajectory model is defined as minimizing acceleration or minimizing control variables. Based on the discrete time step and the state transition equation, the mapping relationship between state variables and control variables at each discrete moment is determined. Subsequently, flight corridor constraints and landing point constraints are introduced into the optimization model to ensure that the UAV's flight trajectory remains safe and feasible throughout the planning space, does not exceed boundaries, and accurately reaches the predetermined endpoint. The flight corridor constraint restricts the UAV's flight path to within a predefined safe area, while the landing point constraint ensures that the terminal state meets the target position and attitude requirements. Furthermore, the solution result of the initial trajectory model can be considered as the UAV's reference trajectory under idealized linear time-invariant conditions. This trajectory will be input into the sequential convex programming module in subsequent steps for iterative correction and optimal solution of the nonlinear model.This hierarchical solution strategy achieves a smooth transition from linear feasible trajectories to nonlinear optimal trajectories, helping to improve the path planning accuracy and computational efficiency of UAVs in complex mission environments. Step S150: Based on the initial trajectory values, use the sequential convex programming algorithm to solve the discretized trajectory planning model to obtain the optimal flight trajectory of the UAV. This step, based on the initial trajectory values ​​obtained in the previous steps, uses the sequential convex programming algorithm to optimize and solve the discretized trajectory planning model, thereby obtaining the optimal flight trajectory of the UAV. The purpose of this step is to further refine the trajectory through iterative optimization based on the given initial values, so that the UAV can minimize control variables or optimize other related objectives, such as flight time or energy consumption, while satisfying system dynamics constraints and mission constraints. Specifically, the initial trajectory value is first input into the sequential convex programming algorithm as the initial solution. The algorithm constructs a convex optimization problem model and uses the state transition equations and control constraints of the discretized trajectory planning model to iteratively solve the problem. In each iteration, the sequential convex programming algorithm linearizes the current solution and continuously adjusts the control quantity by solving the linearized subproblems, so that the UAV trajectory is gradually optimized in each iteration until the convergence criterion is met. By continuously updating the control strategy, the sequential convex programming algorithm ensures that the generated trajectory in each iteration not only meets the constraints of the UAV dynamics model but also can cope with environmental disturbances and external uncertainties. In a more preferred implementation, the algorithm adds a corresponding penalty term to the objective function and constructs a convergence criterion based on this, aiming to minimize the control quantity to prevent unnecessary high energy consumption or excessive control input, ensuring the smoothness and executability of the flight trajectory. Through this optimization process, the algorithm optimizes the trajectory in each iteration, making the UAV's flight path gradually approach the optimal solution. After several iterations, the obtained optimal trajectory can be executed in actual flight, minimizing control energy consumption while maintaining sufficient trajectory accuracy to meet the requirements of the flight mission. The invention will be further illustrated below with a specific example.The dynamics of the UAV are defined as follows: 2 ( ) ( ) ( ) ( ( )) ( ) ( ) ( ) ( ) ( ) ( ) ( ) 1 1 ttt R ttmttt J ttt J t        pvvq T gq ω q ω σ ω ω Ω Where, ( )tp 、 ( )tv 、 ( )tq 、 ( )tω represent the position, velocity, quaternion and angular velocity of the UAV at time t respectively; g represents the gravitational acceleration vector, with [0 0 ]T gg , g represents the gravitational acceleration HK 30137248 A 10 degrees; R represents the rotation matrix; 3J S represents the moment of inertia; m represents the mass of the UAV, ( )tT Let represent the total thrust; σt represents the torque. The total thrust and torque are expressed as: σt = σtMt = σtMt, where σt and Mt represent the configuration parameter matrix of the UAV. Then the nonlinear dynamic equation of the UAV is expressed as: 0 = 0( ) ( ) ( )), ,, ftt ftttt  xxu ( )tx and tu ( ) represent the state variable and control variable under the nonlinear dynamic description of the UAV, respectively. The first trajectory planning model is established with the goal of minimizing the control quantity as follows: 0( , 0 ) min ( )( ) fft fu t tt tw t dt tw t uu Its constraints are:       0 0 0 ( ) ( ( ) ( )) , ( ( ) ( )) , ( ( ) ) , , , 0 , ,, ( ) 0 ghfffnntftttgtttthttt ttttt ttttt tt         xxuxuxu „ ()g and ()h represent the constraint sets, 0 gn and 0 hn are defined as zero vectors, gn and hn respectively represent the constraint set ()g The number of constraints within h, where ( ) ( )), ,gtt tx u is a convex function, ( ) ( )), ,htt tx u is an affine function, and uw and tw represent weight coefficients.Now, we introduce a time mapping operator to transform the above optimization problem from the optimization solution interval [ ]0, ft t with free terminal time to the solution interval [ ]0, f  with fixed terminal time. The mapping relationship is as follows: 0 0 ( ) ( ) , ( ) , ( )ff dt sttttd        Define ( ) ( )tt u , which represents the Euclidean norm, and use the control variables and the time mapping operator to extend the state variables as follows: ( ) [ ( ( )) (, ( )) ( )],T Tt tt    xx Then, for the extended system, its dynamic description is as follows: HK 30137248 A 11 ( ( ) ( )) ( ) ( ) ( ( , , 1 , ) ( )) ( ) ( ) ( ) [ ( ( )) ( )]TT fttd F tsdts                        xuxxxuuuu Based on extended state variables and time mapping, the optimization solution interval of the first trajectory planning model is converted into a fixed terminal time, resulting in the following second trajectory planning model:       ( ) 0 0 0 ( ) ( ) , . . ) , , 0 , , 0 ( ( ( ) ( )) , ,( ( ) ( )) ,( ( ) ( )) ghffffuftnn min wwt F stgh                                uxxuxuxu „ Similarly, ( ( ) ( )), ,g   xu is a convex function, ( ( ) ( )), ,h   xu is an affine function.The linearized expression of the dynamic constraints in the constraint conditions is as follows: ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) ( ) , ( ) ( ) ( ) ( ) ( ) ( ) ( ) , , ABFFABAB                                            zzxxucxuxuxucxu where ( ) [ ( ( ) ( )],)T TT Let   zxu represent the reference trajectory, ( )T x and ( )Tu are the reference values ​​of ( )x and ( )u respectively, ( )A  and ( )B  represent the state matrix and input matrix respectively, and ( )c represent the disturbance term. In order to enable the trajectory planning model to be solved using the sequential convex programming algorithm, it needs to be discretized.This example divides the optimization solution interval into N uniformly distributed discrete points, totaling N-1 subintervals. We define 1 / Nk ∈ [1, Nk]. For each subinterval, the control variable (τu) is represented using the first-order hold interpolation method as follows: [1 1 1 1 1( ) ( ) ( ) , ( )ˆ ) , ( ˆ kkkkkkkkkkkkkkk τ                                          Discretizing the trajectory planning model, the dynamic constraints are transformed into state transition constraints. To address the problem of infeasible solutions caused by linearization, a virtual control term is introduced into the state transition constraints. For each subinterval 1 1 Ni  , the discretized state transition constraints are represented as follows: HK 30137248 A 12 1 1 1 1 1 1 1 1 1 ( ) ( ) ( ) ( ) ( ) ( ) ( ) ) , , , ˆ, , , ( ) ( ) ( ( ) ikikikiiiiiiiiiiioiki iiiiiiiii ABBIA d ABAB d BAB d A d                                                        xxuucvcc Where, I represents the identity matrix, iA represents the state transition matrix, iB and iB represent the control matrices, ic represents the disturbance term, iv represents the virtual control term, and i v.The discretized form of the final trajectory planning model is expressed as follows: \[ \begin{aligned} \underset{z_i, v_{cw}, v_{tr}}{\text{fmin}} \quad & \sum_{i=0}^{n_k} w_i f((w_k)_i, (\tau_k)_i, x_{k,i}, u_{k,i}, v_{z,i}, \zeta_i) + w_{vc}\sum_{i=0}^{n_k} \zeta_i + w_{tr}\xi_k \\ \text{s.t.} \quad & x_{k,i+1} = A_k x_{k,i} + B_k u_{k,i} + f((t_k)_i, x_{k,i}) \\ & g(x_{k,i}, u_{k,i}, \tau_k) \leq 0, \quad i=0, \dots, n_k \\ & h(x_{k, n_k}) = 0 \\ & \|z_k - z_{k-1}\| \leq \xi_k \end{aligned} \] where \(w_{vc}\) and \(w_{tr}\) respectively represent the penalty weights of the virtual control term and the trust region, \(z_k = [x_k^T, u_k^T]^T\) represents the optimized trajectory. The penalty term \(w_{vc}\sum \zeta_i\) in the objective function ensures that the virtual control term is only used when necessary by virtue of its large proportion weight, and the trust region constraint \(\|z_k - z_{k-1}\| \leq \xi_k\) ensures that the solution is valid in the linear domain. For the constraint set \(g_k(x_k, u_k, \tau_k) \leq 0, n_k(0)\), it specifically includes flight corridor constraints, landing point constraints and control variable boundary constraints. The expression of the flight corridor constraint can be expressed as follows: \[ A_c(t)p(t) \leq b_c(t) \] where \(A_c(t)\) and \(b_c(t)\) represent parameters of the flight corridor, and \(p(t)\) represents the position of the UAV at time \(t\). In view of the requirements for safe departure and return, the landing range of the UAV needs to be restricted to ensure that the UAV lands safely at the designated location. Therefore, in this example, a conical region is used to limit the landing range of the UAV, which is regarded as the landing cone constraint, and its linearized expression is expressed as follows: \[ H (p(t) - p_f) \leq e_c \cot \gamma_c \| (p(t) - p_f) \|_K \] HK 30137248 A 13 \[ H = \begin{bmatrix} 1 & 0 & 0 \\ 0 & 1 & 0 \end{bmatrix}, \quad e_c = \begin{bmatrix} 0 \\ 0 \\ 1 \end{bmatrix} \] where \(p_f\) represents the target landing point, and \(\gamma_c\) represents the included angle between the conical region and the horizontal line. The linearized expression of the control variable boundary constraint is defined as follows: \[ u_{min} \leq u(t) \leq u_{max} \] where \(u_{min}\) and \(u_{max}\) respectively represent the lower bound and upper bound of the control variable.The linearized expression of the affine constraint set ( ) ), ,kk kh  xu is defined as follows: 0 0( ) ), ( f ft t  xxxx Since the iterative solution of the sequential convex programming algorithm requires an initial value, in order to maintain convexity, a linear time-invariant system of a UAV with a fixed terminal time is defined for the solution of the initial value as follows: ( ) ( ) ( )t A t B t    xxa ( )ta represents the acceleration of the system, ( )t x represents the state variable of the system. Similarly, the system is discretized, and the optimization solution interval is divided into N discrete points, a total of N-1 sub-intervals, 1 Nk , 5 N Nj  , 1 1 Ni  , and its optimal control problem is defined as follows: 1 ) 0 0 ( ) ) m ( ) ( ) ( ) , ( ) ( ) ( ) ( ) ( ) ( ) ( ( ( in . ( ( . ) ) ) ) kktkkkkkffciicicjfcjf tt A t B tttst A ttbt H t cot t                 aaxxaxxxxpppepp „ „ Solving this optimal control problem yields the initial trajectory values ​​required by the SCP algorithm. Using the SCP algorithm to solve the aforementioned discretized trajectory planning model, the convergence criterion for the algorithm iteration is given as: v tr i imaxi i        vzz „ „ εv and εtr represent the threshold parameters of the convergence criterion for the sequential convex programming algorithm iteration, 1[ , , ]TTTNzzz… , 1[ , , ]TTTNvvv… , ii represents the current iteration round, and imaxi represents the maximum number of iterations.The UAV trajectory planning method provided by the present invention establishes a convex trajectory planning problem, HK 30137248 A 14 By introducing a time mapping operator, the trajectory planning problem under free terminal time is transformed into a fixed terminal time problem, thus fixing the optimization solution interval and avoiding the computational complexity and numerical instability caused by using the terminal time as an optimization variable under free terminal time. This improves the solvability and convergence of the trajectory planning problem. Furthermore, the system state variables are extended based on the time mapping, allowing the extended state variables to simultaneously represent the system state, control variables, and time-varying relationships. This establishes a unified and structured optimal control model under fixed terminal time, providing a good mathematical foundation for subsequent discretization and convex optimization. A better implementation method adds virtual control terms and corresponding penalty terms, ensuring that the linearized optimization problem maintains a feasible solution in each iteration step, avoiding solution failure due to linearization errors, and improving the stability and robustness of the algorithm. The trajectory initial value model provides a good initial point for the SCP algorithm. By simultaneously considering the dynamic constraints, control boundaries, and landing point constraints of the UAV, a dynamically feasible and physically realizable safe flight trajectory is generated in complex environments, making it suitable for autonomous flight control and mission planning of various types of UAVs. The disclosed methods can be implemented using various types of devices. Therefore, this invention also discloses a device corresponding to the above methods, and specific embodiments are given below for detailed description. As shown in Figure 2, one embodiment of this invention provides a UAV trajectory planning device, including: a first modeling module 202, used to construct constraints for the trajectory planning problem based on the dynamic equations of the UAV, and establish a first trajectory planning model with the goal of minimizing the control quantity. The optimization solution interval of the first trajectory planning model is the free terminal time; a second modeling module 204, used to convert the optimization solution interval from the free terminal time to the fixed terminal time using a time mapping operator to obtain a second trajectory planning model; a model discretization module 206, used to discretize the second trajectory planning model to obtain a discretized trajectory planning model; a trajectory initial value solution module 208, used to establish a trajectory initial value model for the UAV with a linear time-invariant system with a fixed terminal time as the object, and solve the trajectory initial value model to obtain the trajectory initial value; and an optimal trajectory solution module 210, used to solve the discretized trajectory planning model using a sequential convex programming algorithm based on the trajectory initial value to obtain the optimal flight trajectory of the UAV. An embodiment of this invention also provides an application of the UAV trajectory planning method proposed in this invention in a UAV cooperative system. The drone collaboration system may include, but is not limited to, multi-drone formation systems, drone swarm systems, drone-ground robot collaboration systems, drone-manned aircraft collaboration systems, and collaboration systems of multiple small drones and drone-mounted platforms.In such collaborative systems, multiple UAVs can generate optimal flight trajectories that conform to their own dynamic characteristics according to the trajectory planning method of this invention. Under the coordination of the task scheduling unit, path decoupling and collaborative control are achieved, enabling the system as a whole to have higher flight safety, energy utilization efficiency, and task completion rate in complex airspace or task scenarios. Furthermore, the trajectory planning method proposed in this invention can also be applied to various collaborative task scenarios such as disaster monitoring, logistics transportation, agricultural and forestry inspection, urban security, and airspace traffic management, enabling UAV swarms to achieve dynamic obstacle avoidance and efficient path allocation in confined environments, thereby further improving the intelligence level and task execution capability of UAV collaborative systems. The device provided in this application embodiment has the same implementation principle and technical effects as the aforementioned method embodiment. For the sake of brevity, any parts not mentioned in the device embodiment can be referred to the corresponding content in the aforementioned method embodiment. The methods and related devices mentioned in the above embodiments are described with reference to the method flowcharts and / or structural diagrams provided in this application embodiment. Specifically, each process and / or block in the method flowchart and / or structural diagram, as well as the combination of processes and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions are provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, create means for implementing the functions specified in one or more flowcharts and / or one or more blocks of a structural diagram. These computer program instructions may also be stored in a computer-readable storage medium capable of directing a computer or other programmable data processing apparatus to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means that implement the functions specified in one or more flowcharts and / or one or more blocks of a structural diagram. These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus to cause a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer-implemented process, such that the instructions, which execute on the computer or other programmable apparatus, provide steps for implementing the functions specified in one or more flowcharts and / or one or more blocks of a structural diagram. The following embodiments illustrate the application of this method to a computer device. It is understood that the computer device can be any device with computing and processing capabilities, including but not limited to a server or a personal laptop computer (HK 30137248 A 16). In one embodiment, the computer device can be an application server, which can be a server used to run the application under test.Referring to Figure 3, which illustrates a hardware block diagram of an electronic device intended to represent various forms of digital computers, such as laptop computers, desktop computers, workstations, personal digital assistants, servers, blade servers, mainframe computers, and other suitable computers. The electronic device may also represent various forms of mobile devices, such as personal digital processors, cellular phones, smartphones, wearable devices, and other similar computing devices. The components shown herein, their connections and relationships, and their functions are merely illustrative and are not intended to limit the implementation of the present application described and / or claimed herein. As shown in Figure 3, the electronic device includes: at least one processor 1, at least one communication interface 2, at least one memory 3, and at least one communication bus 4. In this embodiment, the number of processor 1, communication interface 2, memory 3, and communication bus 4 is at least one, and processor 1, communication interface 2, and memory 3 communicate with each other through communication bus 4. Processor 1 may be a central processing unit (CPU), an application-specific integrated circuit (ASIC), or one or more integrated circuits configured to implement embodiments of the present invention. Memory 3 may include high-speed RAM, and may also include non-volatile memory, such as at least one disk storage device. The memory stores a program, which the processor can call to implement the various processing flows of the aforementioned UAV trajectory planning scheme. This embodiment also provides a readable storage medium storing a computer program, which, when executed by a processor, implements the various processing flows of the UAV trajectory planning scheme provided in any possible implementation of the above embodiments and / or in combination with the embodiments. The present invention has been described in particular detail with respect to possible scenarios in the above embodiments, and those skilled in the art will recognize that the present invention can be practiced through other embodiments. The specific naming of components, the capitalization of terms, attributes, data structures, or any other programming or structural aspects are not mandatory or important, and the mechanisms or features of implementing this invention may have different names, forms, or procedures. The system can be implemented through a combination of hardware and software (as described), entirely through hardware elements or entirely through software elements. The specific division of functions among the various system components described herein is merely exemplary and not mandatory; rather, the functions performed by a single system component can be performed by multiple components, or the functions performed by multiple components can be performed by a single component.Those skilled in the art will understand that the various steps of the disclosed methods can be implemented using general-purpose computing devices. They can be centralized on a single computing device or distributed across a network of multiple computing devices. Optionally, they can be implemented using device-executable program code, allowing them to be stored in a storage device for execution by the computing device, or they can be fabricated as separate integrated circuit modules, or multiple modules or steps can be fabricated as a single integrated circuit module. Thus, the embodiments of the present invention are not limited to any particular combination of hardware and software. These device-executable programs (also referred to as programs, software, software applications, or code) include machine instructions of a programmable processor and can be implemented using high-level procedural and / or object-oriented programming languages, and / or assembly / machine languages. As used herein, the terms "machine-readable medium" and "computer-readable medium" refer to any computer program product, device, and / or apparatus (e.g., disk, optical disk, memory, programmable logic device (PLD)) for providing machine instructions and / or data to a programmable processor, including machine-readable media that receive machine instructions as machine-readable signals. The term "machine-readable signal" refers to any signal used to provide machine instructions and / or data to a programmable processor. Certain aspects of the invention include the process steps and instructions described herein in algorithmic form. It should be noted that the process steps and instructions of the invention can be implemented in software, firmware, and / or hardware, and when implemented in software, they can be downloaded, stored on, and operated from various platforms used by different operating systems. Those skilled in the art will understand that the structures shown in the accompanying drawings are merely block diagrams of some structures related to the present application and do not constitute a limitation on the terminal device to which the present application is applied. Specific terminal devices may include more or fewer components than shown in the figures, or combinations of certain components, or different component arrangements. In the description of this specification, references to terms such as "an embodiment," "some embodiments," "example," "specific example," or "possible design," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example (HK 30137248 A 18) is included in at least one embodiment or example of this application. In this specification, illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in a suitable manner in any one or more embodiments or examples. Moreover, those skilled in the art can combine and integrate the different embodiments or examples described herein, as well as the features of those different embodiments or examples, without contradiction.Finally, it should be noted that in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element. The above embodiments are merely illustrative of the technical solutions of the present invention and are not intended to limit it; although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention. HK 30137248 A Claim 1 1. A method for planning the trajectory of an unmanned aerial vehicle (UAV), characterized by comprising: constructing constraints based on the dynamic equations of the UAV, establishing a first trajectory planning model with the goal of minimizing the control quantity, wherein the optimization solution interval of the first trajectory planning model is the free terminal time; converting the optimization solution interval of the first trajectory planning model from the free terminal time to a fixed terminal time using a time mapping operator to obtain a second trajectory planning model; discretizing the second trajectory planning model to obtain a discretized trajectory planning model; establishing an initial trajectory value model for the UAV using a linear time-invariant system with a fixed terminal time, and solving the initial trajectory value model to obtain initial trajectory values; and solving the discretized trajectory planning model using a sequential convex programming algorithm based on the initial trajectory values ​​to obtain the optimal flight trajectory of the UAV. 2. The method according to claim 1, characterized in that the constraints of the second trajectory planning model include dynamic constraints, and the discretization of the second trajectory planning model to obtain a discretized trajectory planning model includes: dividing the optimization solution interval of the second trajectory planning model into several sub-intervals; for each sub-interval, using the first-order hold interpolation method to discretize the control variables in the dynamic constraints to obtain discretized control variables; and converting the dynamic constraints in the constraints into state transition constraints based on the discretized control variables to complete the discretization of the second trajectory planning model and obtain a discretized trajectory planning model.3. The method according to claim 1, characterized in that, the step of converting the optimization solution interval of the first trajectory planning model from free terminal time to fixed terminal time using a time mapping operator includes: defining the optimization solution interval of the first trajectory planning model as [0, ft t ], where 0t represents the initial time, ft represents the terminal time, and at least one of 0, ft t is an optimization variable; mapping the optimization solution interval [0, ft t] under free terminal time to the optimization solution interval [0, f  ] under fixed terminal time according to the following mapping relationship: 0 0 ( ) ( ) , ( ) , ( )ff dt sttttd        . 4. The method according to claim 1 or 3, characterized in that the process of establishing the second trajectory planning model HK 30137248 A 2 further includes: based on the optimization solution interval with a fixed terminal time, using the time mapping operator and control variables to extend the system state variables of the dynamic equation to obtain extended state variables, wherein the extended state variables simultaneously represent the system state variables, system control variables, and optimization solution interval; and converting the first trajectory planning model into the second trajectory planning model based on the extended state variables and the time mapping operator. 5. The method according to claim 1, characterized in that before solving the discretized trajectory planning model, the method further includes: introducing a virtual control term into the state transition equation in the constraints of the discretized trajectory planning model, and introducing a penalty term corresponding to the virtual control term into the objective function of the discretized trajectory planning model, so that the solution of the discretized trajectory planning model is a feasible solution in the linearization domain. 6. The method according to claim 1, characterized in that, solving the trajectory initial value model to obtain the trajectory initial value includes: defining the optimization objective of the trajectory initial value model as minimizing acceleration, and the constraints include system state transition constraints, flight corridor trajectory point constraints, and landing point constraints; solving the trajectory initial value model to obtain the trajectory initial value.7. A UAV trajectory planning device, characterized in that it comprises: a first modeling module, used to construct constraints based on the dynamic equations of the UAV, and establish a first trajectory planning model with the goal of minimizing the control quantity, wherein the optimization solution interval of the first trajectory planning model is the free terminal time; a second modeling module, used to convert the optimization solution interval of the first trajectory planning model from the free terminal time to a fixed terminal time using a time mapping operator, to obtain a second trajectory planning model; a model discretization module, used to discretize the second trajectory planning model to obtain a discretized trajectory planning model; a trajectory initial value solving module, used to establish a trajectory initial value model for the UAV as a linear time-invariant system with a fixed terminal time, and solve the trajectory initial value model to obtain the trajectory initial value; and an optimal trajectory solving module, used to solve the discretized trajectory planning model using a sequential convex programming algorithm based on the trajectory initial value to obtain the optimal flight trajectory of the UAV. (HK 30137248 A Specification Figure 1: Constructing constraints based on the dynamic equations of the UAV, establishing a first trajectory planning model with the goal of minimizing the control quantity, wherein the optimization solution interval of the first trajectory planning model is the free terminal time.) The optimization solution interval of the first trajectory planning model is transformed from free terminal time to fixed terminal time using a time mapping operator, resulting in the second trajectory planning model. Taking a linear time-invariant system with fixed terminal time as the object, an initial trajectory value model is established for the UAV, and the initial trajectory value is obtained by solving the initial trajectory value model. Based on the initial trajectory value, a sequential convex programming algorithm is used to solve the discrete trajectory planning model to obtain the optimal flight trajectory of the UAV. The second trajectory planning model is discretized to obtain a discretized trajectory planning model. Figure 1: First modeling module, Second modeling module, Model discretization module, Initial trajectory value solution module, Optimal trajectory solution module. Figure 2: HK 30137248 A. 2 Processor, Memory, Program, Communication Interface, Communication Bus. Figure 3: HK 30137248 A.