An optimal trajectory planning method for a quadrotor suspension payload system

Abstract

The present invention relates to the field of quadrotor control and discloses an optimal trajectory planning method for a quadrotor suspension payload system, comprising the following steps: establishing a dynamic model of the quadrotor suspension payload system; estimating the unknown lumped disturbance in the established model using a predefined time nonlinear disturbance observer to obtain a compensated nominal model; minimizing a cost function about path points within the detection range of the quadrotor's onboard sensor using a dynamic window particle swarm optimization algorithm based on the real-time position of the quadrotor to find an initial optimal trajectory; using the compensated nominal model as a new prediction model and the initial optimal trajectory as a reference trajectory, and using nonlinear model predictive control to minimize the cost function to obtain a predicted trajectory that meets system constraints. The method disclosed by the present invention can improve the reliability and stability of load transportation and effectively ensure control accuracy.

Description

Technical Field

[0001] The present invention relates to the field of low-altitude economy, and in particular to an optimal trajectory planning method for a quadrotor suspension payload system. Background Art

[0002] With advances in drone technology, quadrotor systems with suspended payloads have demonstrated significant application value in a variety of fields. By carrying sensors or supplies, these systems enable precise airdrops, equipment deployment, and monitoring in complex environments, overcoming the terrain limitations of traditional operations, improving efficiency, and reducing labor costs. To ensure system stability, PID control, sliding mode control, backstepping control, LQR, and MPC methods are currently the primary methods used. However, these methods have limitations when dealing with strong nonlinear coupling, time-varying disturbances, and actuator constraints.

[0003] Because quadrotor systems with suspended payloads often face uncertainties such as unmodeled dynamics and external disturbances, current approaches to addressing these uncertainties within the nonlinear model predictive control framework include controller robustness, combined adaptive control, and active uncertainty compensation methods. Uncertainty compensation methods, particularly observer-based compensation strategies, have become the primary approach in applications due to their simple structure and computational efficiency. The introduction of sliding mode disturbance observers and extended state observers has effectively improved the system's interference tolerance. However, these methods still suffer from issues such as unstable convergence time, chattering, and steady-state estimation errors.

[0004] Furthermore, it is crucial for the quadrotor system to have real-time autonomous obstacle avoidance capabilities during mission execution. Current obstacle avoidance methods, such as artificial potential fields and the A* algorithm, suffer from local optimality and are difficult to cope with dynamic environments. Summary of the Invention

[0005] To solve the above technical problems, the present invention provides an optimal trajectory planning method for a quadrotor suspension payload system, so as to achieve the purpose of improving the reliability and stability of load transportation.

[0006] To achieve the above object, the technical solution of the present invention is as follows:

[0007] An optimal trajectory planning method for a quadrotor suspension payload system comprises the following steps:

[0008] S1. System modeling: Establish a dynamic model of the quadrotor suspension payload system;

[0009] S2. Disturbance compensation: For the unknown lumped disturbance in the established model, the lumped disturbance is estimated through a predefined time nonlinear disturbance observer to obtain the compensated nominal model;

[0010] S3, trajectory planning: Based on the real-time position of the quadrotor, a dynamic window particle swarm optimization algorithm is used to minimize the cost function of the path points within the detection range of its onboard sensors to find the initial optimal trajectory;

[0011] S4. Optimal control: The compensated nominal model is used as the new prediction model, the initial optimal trajectory is used as the reference trajectory, and nonlinear model predictive control is used to minimize the cost function to obtain a predicted trajectory that satisfies the system constraints.

[0012] In the above scheme, in step S1, the Euler-Lagrangian modeling method is used to establish the quadrotor suspension payload system model as follows:

[0013] ;

[0014] in, is the generalized coordinate, including the position of the quadrotor and posture , and the payload's swing angle ; , Respectively The first and second derivatives of ; is the inertia matrix, represents the Coriolis and centrifugal matrices, is the gravity vector; is the control allocation matrix, is the control matrix, where , indicating the The thrust generated by the propeller, , represents the aerodynamic coefficient, For the The speed of the propeller; is the air resistance, is the air resistance coefficient matrix, is the lumped disturbance vector that includes the unmodeled dynamics as well as the wind disturbance, which is considered as an external disturbance to the system.

[0015] In the above scheme, the specific method of step S2 is as follows:

[0016] First, the dynamic model of the quadrotor suspension payload system is rewritten as follows:

[0017] ;

[0018] Where, is the measurable state of the system, for The first derivative of is the control input of the system, is a diagonal matrix, is the unknown lumped disturbance acting on the system; and About status Nonlinear smooth function of ;

[0019] Then, introduce a new state variable And the auxiliary dynamic system is as follows:

[0020] ;

[0021] in, for The first derivative of represents the difference between two state variables, is the coefficient matrix;

[0022] Finally, the predefined time nonlinear disturbance observer is designed as follows:

[0023] ;

[0024] in, for Estimates, and They are and The first derivative of ; is the Lyapunov function, ; for estimates; is a constant, For predefined time;

[0025] Through the stability analysis based on Lyapunov function, it can be seen that the observation error At a predefined time Converges to 0 internally;

[0026] Finally, define the system state vector as , , , Respectively , and The first derivative of ; Redefine the control vector , using a predefined time nonlinear disturbance observer to estimate the lumped disturbance online , the compensated nominal model is as follows:

[0027] ;

[0028] in, express The first derivative of express Estimates, represents a nonlinear function that describes the dynamics of the system.

[0029] In the above solution, in step S3, the cost function of the path point is as follows:

[0030] ;

[0031] in, 、 、 、 、 、 Represent the cost functions for obstacle avoidance, equidistant planning, smooth path, minimizing path length, adaptive planning, and smooth climbing respectively; is the weight corresponding to the above cost function; Indicates the number of obstacles within the sensor's detection range.

[0032] In the above scheme, in step S3, according to the cost function , the path planning problem is transformed into an optimization problem, and the dynamic window particle swarm optimization algorithm is used to minimize , to find all optimal path points, the specific process is as follows:

[0033] (1) Cost evaluation: Each particle calculates the path cost based on its current position, using a predefined cost function Quantify its advantages and disadvantages;

[0034] (2) Individual and group collaboration: Each particle records its historical optimal position , that is, the path point with the lowest cost in its own exploration. By comparing the costs of all particles, the global optimal position shared by the group can be found. , which is the lowest cost path point found among all particles currently;

[0035] (3) Position update: Particles are updated by combining and Dynamically adjust particle speed and position based on the information to balance local exploration and global convergence;

[0036] (4) Iterative optimization: Repeat the above process (1)-(3), the particle swarm gradually approaches the global minimum of the cost function, and finally outputs the optimal path point sequence.

[0037] In the above scheme, in step S3, the particle velocity and position are updated as follows:

[0038] ;

[0039] in, For particles In time The speed of For particles In time The speed of For particles In time The position at the time, For particles In time The position at the time, is the coefficient of inertia; , are the individual and global acceleration coefficients, respectively; , for Two random values in the range.

[0040] In the above solution, in step S3, the search space of the particle swarm is limited to the detection range of the sensor, thereby realizing local particle swarm optimization, that is, dynamic window particle swarm optimization algorithm.

[0041] In the above solution, the specific method of step S4 is as follows:

[0042] The continuous-time optimal control problem is transformed into a discrete optimization problem using the multi-shooting technique, and the explicit Euler method is used with a sampling period Perform numerical integration to obtain a discrete prediction model , , , for The state, control quantity and disturbance estimation at time and prediction time domain In it, the following nonlinear programming problem is constructed, that is, minimizing the cost function:

[0043] ;

[0044] in, , Respectively represent the set of states and control quantities in the prediction time domain;

[0045] Item 1 is the trajectory tracking term, defined as follows:

[0046] ;

[0047] in, 、 、 、 Respectively The position, speed, attitude angle and attitude angular velocity of the quadrotor at all times; , Indicates the swing angle and swing angular velocity of the payload; For the moment Reference trajectory, superscript represents "reference", which is generated by the trajectory planning module; 、 、 、 、 、 are the weight matrices for the quadrotor position, quadrotor velocity, quadrotor attitude angle, quadrotor attitude angular velocity, payload swing angle, and payload swing angular velocity, respectively; the symbols are Represents a vector About Matrix The weighted square norm of ;

[0048] Item 2 To control the smoothing term, as follows:

[0049] ;

[0050] in, For the quadrotor at time Controlled output; For the moment Expected control quantity, superscript Represents "reference", defined as the control input of the hover state; For the quadrotor at time The amount of control; , are the weight matrices for control quantity and control smoothness respectively;

[0051] Item 3 is the active obstacle avoidance item, which includes the quadrotor obstacle avoidance cost and the payload obstacle avoidance cost:

[0052] ;

[0053] in, It is a collection of obstacles within the detection range of the quadrotor sensor; and is the smoothness parameter with respect to the quadrotor and payload; For the quadcopter to obstacles The Euclidean distance of For payload to obstacle The Euclidean distance of and are the obstacle avoidance radius of the quadrotor and payload, respectively;

[0054] Item 4 is the terminal cost function, and its weight matrix is ,use" "Unified representation of subscript, meet , ensuring the convergence of the terminal state in the prediction time domain.

[0055] In the above scheme, the system constraints in step S4 are as follows:

[0056] ;

[0057] ;

[0058] ;

[0059] ;

[0060] in, is the initial condition, , They represent the feasible regions of system state and control input respectively, which are expressed as follows:

[0061] ;

[0062] ;

[0063] express dimensional real space, is the maximum speed, The upper limit of thrust provided by each propeller.

[0064] In the above scheme, in step S4, the solution is obtained through the sequential quadratic programming framework, in which the Gauss-Newton method is used to transform the nonlinear programming problem into a series of quadratic optimization sub-problems, and the qpOASES solver is used for calculation. At the same time, the real-time iteration strategy and hot start technology are combined to ensure the real-time performance of the calculation. Finally, the ACADO tool chain is used to realize the automated process from modeling to code generation, and high-precision trajectory tracking control in complex disturbance environments is completed.

[0065] Through the above technical solution, the optimal trajectory planning method for a quadrotor suspension payload system provided by the present invention has the following beneficial effects:

[0066] 1. This invention adopts an energy-based Euler-Lagrangian modeling method and achieves the coordinated optimization of load swing suppression and dynamic obstacle avoidance through the system's coupled dynamic characteristics, providing an efficient and reliable solution for application scenarios such as low-altitude logistics transportation;

[0067] 2. This invention combines a predefined-time disturbance observer with a nonlinear model predictive control framework, significantly improving the system's anti-interference capability. Theoretical evidence demonstrates that the predefined-time disturbance observer can estimate the lumped disturbance within a preset time without any steady-state error. The compensated model is then used in nonlinear model predictive control optimization, effectively ensuring control accuracy.

[0068] 3. This paper proposes a two-stage optimal trajectory planning strategy based on dynamic window particle swarm optimization and nonlinear model predictive control. First, a dynamic window particle swarm optimization algorithm is used to generate a local reference trajectory that meets the requirements within the local perception range. Subsequently, nonlinear model predictive control is used to dynamically optimize the trajectory. Ultimately, this strategy achieves real-time optimal trajectory planning for a quadrotor system with a suspended payload in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0069] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for describing the embodiments or the prior art.

[0070] Figure 1 A schematic diagram of a quadrotor payload suspension system disclosed in an embodiment of the present invention;

[0071] Figure 2 This is a structural diagram of the overall control system of a quadrotor suspension payload system disclosed in an embodiment of the present invention;

[0072] Figure 3 A schematic diagram of an obstacle avoidance cost function disclosed in an embodiment of the present invention;

[0073] Figure 4 A schematic diagram of an equidistance planning cost function disclosed in an embodiment of the present invention;

[0074] Figure 5 A schematic diagram of a smooth path cost function disclosed in an embodiment of the present invention;

[0075] Figure 6 A schematic diagram of a path length minimization cost function disclosed in an embodiment of the present invention;

[0076] Figure 7 A schematic diagram of an adaptive planning cost function disclosed in an embodiment of the present invention;

[0077] Figure 8 A schematic diagram of a smooth climbing cost function disclosed in an embodiment of the present invention;

[0078] Figure 9 A schematic diagram of a two-stage optimal trajectory planning disclosed in an embodiment of the present invention. DETAILED DESCRIPTION

[0079] The technical solutions in the embodiments of the present invention will be described clearly and completely below with reference to the accompanying drawings in the embodiments of the present invention.

[0080] This paper provides an optimal trajectory planning method for a quadrotor suspended payload system. Through disturbance compensation using a predefined time disturbance observer and trajectory optimization using dynamic window particle swarm optimization, it achieves stable control and trajectory planning in complex environments. Its innovative nonlinear model predictive control framework supports real-time obstacle avoidance and strong wind resistance, significantly improving transportation efficiency compared to manual operations. It can be widely used in high-precision lifting scenarios such as power inspections and high-altitude rescue operations, providing a safe and reliable solution for intelligent UAV lifting.

[0081] S1. System modeling: Establish a dynamic model of the quadrotor suspension payload system.

[0082] Schematic diagram of the quadrotor suspension payload system Figure 1 As shown; the quadrotor shape adopts an "X" configuration; in order to conveniently describe the motion state of the system, an inertial coordinate system is defined As a global reference frame, it always remains stationary; Represents the body coordinate system, which is fixed to the quadrotor fuselage and moves with the quadrotor. Its origin Coincident with the center of mass of the quadrotor; in addition, a payload coordinate system is established , its origin Coincident with the center of mass of the quadrotor, its direction is always consistent with the inertial coordinate system Parallel; Next, we can use the inertial coordinate system The six degrees of freedom of the quadrotor are defined as and , respectively represent its position and attitude vector; define For payload in coordinate system The swing angle vector in is equivalent to the inertial coordinate system The swing angle vector in the payload is respectively and Formed by counterclockwise rotation;

[0083] The quadrotor suspension payload system has 8 degrees of freedom, and the generalized coordinates are selected as , according to the Euler-Lagrange equation, the dynamic equation of the system is expressed as follows:

[0084] ;

[0085] in, , Respectively The first and second derivatives of ; is the inertia matrix, represents the Coriolis and centrifugal matrices, is the gravity vector; is the control allocation matrix, is the control matrix, where , indicating the The thrust generated by the propeller, , represents the aerodynamic coefficient, For the The speed of the propeller; is the air resistance, is the air resistance coefficient matrix, is the lumped disturbance vector that includes the unmodeled dynamics as well as the wind disturbance, which is considered as an external disturbance to the system.

[0086] S2. Disturbance compensation: For the unknown lumped disturbance in the constructed model, the lumped disturbance is estimated through a predefined time nonlinear disturbance observer to obtain the compensated nominal model.

[0087] The overall structure of the system is as follows Figure 2 As shown in the figure, a local reference trajectory is first obtained through a trajectory planning module based on a dynamic window particle swarm optimization algorithm, which is then further optimized through a nonlinear model predictive control module. Finally, the optimal control variable is input into the quadrotor suspension payload system to obtain the optimal trajectory that meets the system constraints and obstacle avoidance requirements. In order to enhance the system's anti-interference ability, a predefined time disturbance observer is introduced to estimate the lumped disturbance, and the nominal model after disturbance compensation is used as the prediction model for the nonlinear model predictive control, ensuring that the system can maintain good performance and stability in the face of external interference and uncertainty.

[0088] First, the dynamic model of the quadrotor suspension payload system is rewritten as follows:

[0089] ;

[0090] Where, is the measurable state of the system, for The first derivative of is the control input of the system, is a diagonal matrix, is the unknown lumped disturbance acting on the system; and About status Nonlinear smooth function of ;

[0091] Then, introduce a new state variable And the auxiliary dynamic system is as follows:

[0092] ;

[0093] in, for The first derivative of represents the difference between two state variables, is the coefficient matrix;

[0094] Finally, the predefined time nonlinear disturbance observer is designed as follows:

[0095] ;

[0096] in, for Estimates, and They are and The first derivative of ; is the Lyapunov function, ; for estimates; is a constant, For predefined time;

[0097] Through the stability analysis based on Lyapunov function, it can be seen that the observation error At a predefined time Converges to 0 internally;

[0098] Finally, define the system state vector as , , , Respectively , and The first derivative of ; Redefine the control vector , using a predefined time nonlinear disturbance observer to estimate the lumped disturbance online , the compensated nominal model is as follows:

[0099] ;

[0100] in, express The first derivative of express Estimates, represents a nonlinear function that describes the dynamics of the system.

[0101] S3. Trajectory planning: Based on the real-time position of the quadrotor, a dynamic window particle swarm optimization algorithm is used to minimize the cost function of the path points within the detection range of the sensor on board to find the initial optimal trajectory.

[0102] The path planning problem is formulated as a linear combination of cost functions that integrate the optimal trajectory criteria and the quadrotor's obstacle avoidance constraints. A dynamic window particle swarm optimization algorithm is then used to minimize the total cost to find the optimal trajectory. It is worth noting that to ensure the smoothness of the reference trajectory, a cubic spline curve is used to generate a smooth trajectory that passes through all path points.

[0103] 1. Obstacle avoidance: Figure 3 As shown, let the obstacle avoidance radius of each particle be , the position of the new path point to be solved is ; Surround obstacles The center coordinates of the smallest cylinder are , the radius is The desired safety distance is , is the distance between the new path point and the obstacle; the obstacle avoidance cost function is defined as follows:

[0104] .

[0105] 2. Equidistant planning: In order to reach the target point with the least number of planning times, it is necessary to define a lower limit for the distance between two adjacent path points. At the same time, in order to avoid crossing obstacles due to the distance between two adjacent path points being too large, resulting in the loss of obstacle avoidance effect, it is necessary to define an upper limit. In short, path points need to be evenly distributed to achieve a reasonable distribution. The distance range of adjacent path points is specified as , ,in , distance parameter satisfy ; Let the position of the current path point be ,definition ,in and express and The two-dimensional plane component of . The related cost function is defined as:

[0106] ;

[0107] like Figure 4 As shown, assuming 、 and are the positions of three candidate particles, of which only It meets the planned distance requirement, so it is used as a new waypoint.

[0108] 3. Smooth path: In order to suppress the swing of the payload, the reference path is required to be as smooth as possible; the previous path point is represented as ,definition for the reason point to vector, for the reason point to vector; then, the angle between the two vectors determines the smoothness of the path, which is expressed as follows:

[0109] ;

[0110] in, ;like Figure 5 As shown, assuming and are two possible positions of the new path point, and the corresponding smoothness cost is and , if present , then the next path point is selected first .

[0111] 4. Minimize path length: To improve task execution efficiency, the path length needs to be shortened as much as possible. In an unknown environment, try to move along the line between the starting point and the target point. This usually results in a shorter path and avoids going further and further. Assuming the coordinates of the starting point and the target point are and , minimize and The angle between them ensures that the new path point is always as close to the straight line as possible. , the relevant cost function is expressed as follows:

[0112] ;

[0113] like Figure 6 As shown, due to , so choose is the new waypoint.

[0114] 5. Adaptive planning: There are often various external interferences in the actual environment, which causes the quadrotor to not always accurately track the reference trajectory and may deviate from the feasible trajectory; therefore, it is necessary to perform adaptive planning. Even if the quadrotor deviates from the current trajectory, it can be replanned and continue to guide the quadrotor to move towards the target point. Assume that the current position of the quadrotor is , adaptive programming aims to generate the closest line path points, thereby expanding the system's attraction domain and enhancing robustness; adaptive planning can be achieved by minimizing the vector and The angle between them is realized:

[0115] ;

[0116] like Figure 7 As shown, according to Priority will be given to as a new waypoint.

[0117] 6. Smooth climb: In order to quickly pull up the payload, the trajectory in the initial stage needs to rise quickly. In addition, considering the requirement of smoothness of the trajectory, the vertical trajectory is finally selected to be a quadratic curve shape; definition ,in and express as well as The vertical component of ; the smooth climb requirement is defined by the following cost function:

[0118] ;

[0119] function The definition is as follows:

[0120] ;

[0121] Among them, the angle for and Angle:

[0122] ;

[0123] The symbol " " indicates a general placeholder; e.g. Figure 8 As shown, located and The curve segment between represents the projection of the reference trajectory on the two-dimensional plane, the planned part is represented by a solid line, and the unplanned part is represented by a dotted line; and The quadratic curve between represents the fitting curve in the vertical direction of the reference trajectory, which takes into account both smoothness and climbing requirements;

[0124] The total cost function is the weighted sum of all cost functions, expressed as follows:

[0125] ;

[0126] in, Indicates the number of obstacles within the sensor detection range, is the weight coefficient.

[0127] According to the cost function , the path planning problem is transformed into an optimization problem, and the dynamic window particle swarm optimization algorithm is used to minimize , to find all optimal path points, the specific process is as follows:

[0128] (1) Cost evaluation: Each particle calculates the path cost based on its current position, using a predefined cost function Quantify its advantages and disadvantages;

[0129] (2) Individual and group collaboration: Each particle records its historical optimal position , that is, the path point with the lowest cost in its own exploration. By comparing the costs of all particles, the global optimal position shared by the group can be found. , which is the lowest cost path point found among all particles currently;

[0130] (3) Position update: Particles are updated by combining and Dynamically adjust particle speed and position based on the information to balance local exploration and global convergence;

[0131] (4) Iterative optimization: Repeat the above process (1)-(3), the particle swarm gradually approaches the global minimum of the cost function, and finally outputs the optimal path point sequence.

[0132] The particle speed and position are updated as follows:

[0133] ;

[0134] in, For particles In time The speed of For particles In time The speed of For particles In time The position at the time, For particles In time The position at the time, is the coefficient of inertia; , are the individual and global acceleration coefficients, respectively; , for Two random values in the range.

[0135] Traditional particle swarm optimization algorithms usually require input of the entire map information during the initialization phase. Although this can achieve better search results, it relies on global information and is not suitable for unknown environments. In actual situations, the detection range of the sensors on the quadrotor is limited, that is, usually only map information of a certain area around the quadrotor can be obtained, so the traditional particle swarm optimization algorithm will no longer be applicable. By abstracting the sensor detection range into a window that moves with the quadrotor and limiting the search space of the particle swarm to it, the particle swarm optimization algorithm will only search in the current known area, thereby achieving local particle swarm optimization, that is, the dynamic window particle swarm optimization algorithm. Figure 9 As shown in the figure, the quadrotor only detects obstacles within the window and performs local path planning; the black dot in the figure represents the optimal particle position, the black solid line on the left side of the quadrotor represents the historical optimal trajectory, and the black solid line on the right side of the quadrotor is the local optimal trajectory, which is used as a reference trajectory.

[0136] S4. Optimal control: The compensated nominal model is used as the new prediction model, the initial optimal trajectory is used as the reference trajectory, and nonlinear model predictive control is used to minimize the cost function to obtain a predicted trajectory that satisfies the system constraints.

[0137] The continuous-time optimal control problem is transformed into a discrete optimization problem using the multi-shooting technique, and the explicit Euler method is used with a sampling period Perform numerical integration to obtain a discrete prediction model , , , for The state, control quantity and disturbance estimation at time and prediction time domain In the example, the following nonlinear programming problem is constructed:

[0138] ;

[0139] The system constraints are as follows:

[0140] ;

[0141] ;

[0142] ;

[0143] ;

[0144] in, , Respectively represent the set of states and control quantities in the pre-test domain, is the initial condition, , They represent the feasible regions of system state and control input respectively, which are expressed as follows:

[0145] ;

[0146] ;

[0147] express dimensional real space, is the maximum speed, The upper limit of thrust provided by each propeller;

[0148] Item 1 is the trajectory tracking term, defined as follows:

[0149] ;

[0150] in, 、 、 、 Respectively The position, speed, attitude angle and attitude angular velocity of the quadrotor at all times; , Indicates the swing angle and swing angular velocity of the payload; For the moment Reference trajectory, superscript represents "reference", which is generated by the trajectory planning module; 、 、 、 、 、 are all weight matrices, and their subscripts are used to distinguish different matrices; the symbol Represents a vector About Matrix The weighted square norm of ;

[0151] Item 2 To control the smoothing term, as follows:

[0152] ;

[0153] in, For the quadrotor at time Controlled output; For the moment Expected control quantity, superscript Represents "reference", defined as the control input of the hover state; For the quadrotor at time The amount of control; , is the weight matrix, and its subscript is used to distinguish different matrices;

[0154] Item 3 is the active obstacle avoidance item, which includes the quadrotor obstacle avoidance cost and the payload obstacle avoidance cost:

[0155] ;

[0156] in, It is a collection of obstacles within the detection range of the quadrotor sensor; , is the smoothness parameter with respect to the quadrotor and payload; For the quadcopter to obstacles The Euclidean distance of For payload to obstacle The Euclidean distance of , are the obstacle avoidance radius of the quadrotor and payload, respectively;

[0157] Item 4 is the terminal cost function, and its weight matrix is ,use" "Unified representation of subscript, meet , ensuring the convergence of the terminal state in the prediction time domain.

[0158] The nonlinear programming problem is solved using a sequential quadratic programming framework. The Gauss-Newton method is used to transform the nonlinear programming problem into a series of quadratic optimization subproblems, which are then calculated using the qpOASES solver. Real-time iteration strategies and hot start techniques are combined to ensure real-time computation. The ACADO tool chain is used to automate the process from modeling to code generation, enabling high-precision trajectory tracking control in complex disturbance environments.

[0159] The local reference trajectory generated in S3 is used as the reference trajectory for nonlinear model predictive control, and then the predicted trajectory is obtained by minimizing the cost function, as shown in Figure 9 The black dotted line on the right side of the quadrotor is shown; the predicted trajectory needs to comprehensively consider obstacle avoidance, payload sway reduction, wind disturbance resistance, and smooth control requirements, so it usually cannot coincide with the local reference trajectory. The trajectory tracking-related costs can be appropriately relaxed to allow a certain tracking error; by combining the trajectory optimization stage based on the dynamic window particle swarm optimization algorithm with the optimal control stage based on nonlinear model predictive control, a two-stage optimal trajectory planning is finally achieved.

[0160] The above description of the disclosed embodiments is intended to enable one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be readily apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention is not limited to the embodiments shown herein but is intended to conform to the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. An optimal trajectory planning method for a quadrotor suspension payload system, characterized in that: The steps include: S1. System modeling: Establish a dynamic model of the quadrotor suspension payload system; S2. Disturbance compensation: For the unknown lumped disturbance in the established model, the lumped disturbance is estimated through a predefined time nonlinear disturbance observer to obtain the compensated nominal model; S3, trajectory planning: Based on the real-time position of the quadrotor, a dynamic window particle swarm optimization algorithm is used to minimize the cost function of the path points within the detection range of its onboard sensors to find the initial optimal trajectory; S4, optimal control: Using the compensated nominal model as the new prediction model and the initial optimal trajectory as the reference trajectory, nonlinear model predictive control is used to minimize the cost function to obtain a predicted trajectory that satisfies the system constraints; The specific method of step S2 is as follows: First, the dynamic model of the quadrotor suspension payload system is rewritten as follows: Where, X=[ξ T ,η T ,μ T ] T is the measurable state vector of the system, is the first-order derivative of the state vector X, U is the control input vector of the system, C is a diagonal matrix, and D is the lumped disturbance vector; F(X) and G(X) are nonlinear smooth functions about the state vector X; Then, a new state variable Ξ and the auxiliary dynamic system are introduced as follows: in, is the first-order derivative of Ξ, Π=X-Ξ represents the difference between two state variables, and Γ is the coefficient matrix; Finally, the predefined time nonlinear disturbance observer is designed as follows: in, is the estimate of π, and are π and The first derivative of ; is the Lyapunov function, is the estimated value of the lumped disturbance vector D; Υ∈(0,1) is a constant, T is a predefined time; Through the stability analysis based on Lyapunov function, it can be seen that the observation error Will be bounded in time Converges to 0 internally; Finally, define the system state vector as denote the first-order derivatives of ξ, η, and μ, respectively; the control input vector u = U is redefined, and the lumped disturbance vector d = D is estimated online using a predefined time nonlinear disturbance observer. The nominal model after compensation is as follows: in, represents the first-order derivative of the state vector x, represents the estimated value of the lumped disturbance vector d, and f represents the nonlinear function describing the system dynamics; The specific method of step S4 is as follows: The continuous-time optimal control problem is transformed into a discrete optimization problem using the multi-shooting technique, and the explicit Euler method is used with a sampling period T s Perform numerical integration to obtain a discrete prediction model x k ,u k , are the estimated values of the state vector, control input vector, and lumped disturbance vector at time k; F represents the nonlinear function describing the dynamics of the discretized system; at time k and within the prediction horizon N, the following nonlinear programming problem is constructed, i.e., minimizing the cost function: in, Respectively represent the set of state vectors and control input vectors in the prediction domain; The first item J x (x i ) is the trajectory tracking term, which is defined as follows: Among them, ξ k+i|k 、 η k+i|k 、 They represent the position, velocity, attitude angle and attitude angular velocity of the quadrotor at time k+i respectively; μ k+i|k , Indicates the swing angle and swing angular velocity of the payload; is the reference position at time k+i, where the superscript r indicates "reference", and is generated by the trajectory planning module; are the weight matrices for the quadrotor position, quadrotor velocity, quadrotor attitude angle, quadrotor attitude angular velocity, payload swing angle, and payload swing angular velocity, respectively; the symbols are represents the weighted square norm of vector m with respect to matrix h; The second item J u (u i ) is the control smoothing term, as shown below: Among them, u k+i|k is the control input vector of the quadrotor at time k+i; is the reference control input vector at time k+i, where the superscript r represents “reference” and is defined as the control input vector of the hovering state; u k+i-1|k is the control input vector of the quadrotor at time k+i-1; are the weight matrices for the control input vector and control smoothness respectively; The third item J o (x i ) is the active obstacle avoidance term, which includes the quadrotor obstacle avoidance cost and the payload obstacle avoidance cost: in, is the set of obstacles within the detection range of the quadrotor sensor; σ q and σ p is the smoothness parameter about the quadrotor and payload; d q,a For the quadrotor to obstacle O a The Euclidean distance, d p,b For payload to obstacle O b The Euclidean distance; subscripts a and b represent the identifiers of obstacles. When a and b are equal, it means that the quadrotor and payload are affected by the same obstacle; r q and r p are the obstacle avoidance radius of the quadrotor and payload, respectively; Item 4J f (x N ) is the terminal cost function, which ensures the convergence of the terminal state in the prediction time domain.

2. The optimal trajectory planning method for a quadrotor suspension payload system according to claim 1, characterized in that: In step S1, the Euler-Lagrange modeling method is used to establish the quadrotor suspension payload system model as follows: Where, q = [ξ T ,η T ,μ T ] T is the generalized coordinate, including the position ξ and attitude η of the quadrotor, and the swing angle μ of the payload; denote the first and second order derivatives of q respectively; M(q) is the inertia matrix, represents the Coriolis and centrifugal matrices, G(q) is the gravity vector; B is the control allocation matrix, U=[f1,f2,f3,f4] T is the control input vector, where represents the thrust generated by the i-th propeller, i = 1,…,4, C T represents the aerodynamic coefficient, is the rotation speed of the i-th propeller; is the air resistance, K is the air resistance coefficient matrix, and D is the lumped disturbance vector including unmodeled dynamics and wind disturbances, referred to as the lumped disturbance vector.

3. The optimal trajectory planning method for a quadrotor suspension payload system according to claim 1, characterized in that: In step S3, the cost function of the path point is as follows: Among them, F1, F2, F3, F4, F5, and F6 represent the cost functions for obstacle avoidance, equidistant planning, smooth path, minimizing path length, adaptive planning, and smooth climbing, respectively; Δ i >0 is the weight corresponding to the above cost function; represents the number of obstacles within the detection range of the sensor; o represents the obstacle identifier, which is used to distinguish different obstacles.

4. The optimal trajectory planning method for a quadrotor suspension payload system according to claim 3, characterized in that: In step S3, the path planning problem is transformed into an optimization problem based on the cost function F. The dynamic window particle swarm optimization algorithm is used to minimize F to find all the optimal path points. The specific process is as follows: (1) Cost evaluation: Each particle calculates the path cost based on its current position, and its quality is quantified by a predefined cost function F; (2) Individual and group collaboration: Each particle records its historical optimal position p i , that is, the path point with the lowest cost in its own exploration. By comparing the costs of all particles, the group can find the global optimal position p shared by the group. g , which is the lowest cost path point found among all particles currently; (3) Position update: Particles are updated by combining p i and p g Dynamically adjust particle speed and position based on the information to balance local exploration and global convergence; (4) Iterative optimization: Repeat the above process (1)-(3), the particle swarm gradually approaches the global minimum of the cost function, and finally outputs the optimal path point sequence.

5. The optimal trajectory planning method for a quadrotor suspension payload system according to claim 4, characterized in that: In step S3, the particle velocity and position are updated as follows: Among them, v i (t) is the velocity of particle i at time t, v i (t+1) is the velocity of particle i at time t+1, x i (t+1) is the position of particle i at time t+1, x i (t) is the position of particle i at time t, w is the inertia coefficient; c1, c2 are the individual and global acceleration coefficients respectively; r1, r2 are two random values in the range (0,1).

6. The optimal trajectory planning method for a quadrotor suspension payload system according to claim 1, characterized in that: In step S3, the search space of the particle swarm is limited to the detection range of the sensor, thereby realizing local particle swarm optimization, namely, the dynamic window particle swarm optimization algorithm.

7. The optimal trajectory planning method for a quadrotor suspension payload system according to claim 1, characterized in that: The system constraints in step S4 are as follows: x0=x(t0) Among them, x k+i ,u k+i , are the estimated values of the state vector, control input vector and lumped disturbance vector at time k+1 respectively; x(t0) is the initial condition, They represent the feasible regions of the system state and control input vector respectively, which are expressed as follows: represents the s-dimensional real space, v max is the maximum speed, f max The upper limit of thrust provided by each propeller.

8. The optimal trajectory planning method for a quadrotor suspension payload system according to claim 1, characterized in that: In step S4, the solution is obtained through a sequential quadratic programming framework, in which the Gauss-Newton method is used to transform the nonlinear programming problem into a series of quadratic optimization subproblems, and the qpOASES solver is used for calculation. At the same time, the real-time calculation is guaranteed by combining real-time iteration strategy and hot start technology. Finally, the ACADO tool chain is used to realize the automated process from modeling to code generation, completing high-precision trajectory tracking control in complex disturbance environments.

Images