Optimal trajectory planning method for four-rotor suspension payload system

Abstract

The invention relates to the field of four-rotor control, and discloses an optimal trajectory planning method for a four-rotor suspension payload system, which comprises the following steps of: establishing a four-rotor suspension payload system kinetic model; for unknown lumped disturbance in the established model, estimating the lumped disturbance through a predefined time nonlinear disturbance observer to obtain a compensated nominal model; based on the real-time positions of the four rotors, a dynamic window particle swarm optimization algorithm is adopted in the detection range of a sensor carried by the four rotors to minimize a cost function about path points, and an initial optimal track is found; and taking the compensated nominal model as a new prediction model, taking the initial optimal trajectory as a reference trajectory, and using nonlinear model prediction control to minimize a cost function so as to obtain a prediction trajectory meeting system constraints. According to the method disclosed by the invention, the load transportation reliability and stability can be improved, and the control precision is effectively ensured.

Description

Technical Field

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

[0002] With the progress of unmanned aerial vehicle technology, the quadrotor suspended payload system has demonstrated important application value in multiple fields. By carrying sensors or supplies, this system can perform precise airdrops, equipment deployments, and monitoring tasks in complex environments, overcoming the terrain limitations of traditional operations, improving work efficiency, and reducing labor costs. To ensure the stability of the system, methods such as PID control, sliding mode control, backstepping control, and LQR, MPC are currently mainly used. However, these methods have certain limitations when dealing with strong nonlinear couplings, time-varying disturbances, and actuator constraints.

[0003] Since the quadrotor suspended payload system usually faces uncertainties such as unmodeled dynamics and external disturbances, current approaches to handling these uncertainties in the nonlinear model predictive control framework include controller robustness, combining adaptive control, and active uncertainty compensation methods. Uncertainty compensation methods, especially observer-based compensation strategies, have become the main solutions in applications due to their simple structure and high computational efficiency. By introducing sliding mode disturbance observers, extended state observers, etc., the anti-interference ability of the system has been effectively improved. However, these methods still have problems such as unstable convergence time, chattering phenomenon, and steady-state estimation error.

[0004] In addition, it is also crucial for the quadrotor suspended payload system to have the ability of real-time autonomous obstacle avoidance during the mission execution. Current obstacle avoidance methods, such as artificial potential field and A* algorithm, have problems of local optimality and difficulty in dealing 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 suspended payload system to achieve the purpose of improving the reliability and stability of load transportation.

[0006] To achieve the above purpose, the technical solution of the present invention is as follows: An optimal trajectory planning method for a quadrotor suspended payload system, comprising the following steps: S1. System modeling: Establish a dynamic model of the quadrotor suspended payload system; S2. Disturbance compensation: For the unknown lumped disturbance in the established model, estimate the lumped disturbance through a predefined time nonlinear disturbance observer to obtain a compensated nominal model; S3. Trajectory Planning: Based on the real-time position of the quadrotor, within the detection range of the sensors it carries, the dynamic window particle swarm optimization algorithm is used to minimize the cost function with respect to the path points 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, thereby obtaining the predicted trajectory that satisfies the system constraints.

[0007] In the above solution, in step S1, the Euler-Lagrange modeling method is adopted to establish the quadrotor suspended payload system model as follows: ; Wherein, is the generalized coordinate, including the position and attitude of the quadrotor, as well as the swing angle of the payload; , respectively represent the first and second derivatives of; is the inertia matrix, represents the Coriolis and centrifugal matrix, is the gravity vector; is the control allocation matrix, is the control matrix, where , represents the thrust generated by the th propeller, , represents the aerodynamic coefficient, is the th propeller speed; is the air resistance, is the air resistance coefficient matrix, is the lumped disturbance vector containing the unmodeled dynamics and wind disturbances, which is regarded as the external disturbance of the system.

[0008] In the above solution, the specific method of step S2 is as follows: First, rewrite the quadrotor suspended payload system dynamics model into the following expression: ; In the formula, is the measurable state of the system, is the first derivative of, is the control input of the system, is the diagonal matrix, is the unknown lumped disturbance acting on the system; and are with respect to the state nonlinear smooth function; Then, introduce a new state variable and the auxiliary dynamic system as follows: ; where, is the first derivative of, represents the difference between two state variables, is the coefficient matrix; Finally, design the predefined-time nonlinear disturbance observer as follows: ; where, is the estimation of, and are respectively and the first derivatives of; is the Lyapunov function, ; is the estimation of; is a constant, is the predefined time; Through the stability analysis based on the Lyapunov function, it can be known that the observation error will converge to 0 within the predefined time ; Finally, define the system state vector as , , , respectively represent , and the first derivatives of; redefine the control vector , and use the predefined-time nonlinear disturbance observer to online estimate the lumped disturbance , and the compensated nominal model obtained is as follows: ; where, represents the first derivative of, represents the estimation of, represents the nonlinear function describing the system dynamics.

[0009] In the above scheme, in step S3, the cost function for the path points is as follows: ; where, , , , , , respectively represent the cost functions for obstacle avoidance, equidistant planning, smooth path, minimizing path length, adaptive planning, and smooth climbing; is the weight corresponding to the above cost function; represents the number of obstacles within the detection range of the sensor.

[0010] In the above solution, 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: (1) Cost evaluation: Each particle calculates the path cost based on its current position, and the predefined cost function quantifies its quality; (2) Individual and group cooperation: Each particle records its historical optimal position , that is, the path point with the minimum cost in its own exploration. By comparing the costs of all particles, the group can find the globally shared optimal position , that is, the path point with the lowest cost found among all current particles; (3) Position update: The particle dynamically adjusts its velocity and position by combining and information to balance local exploration and global convergence; (4) Iterative optimization: Repeat the above processes (1)-(3). The particle swarm gradually approaches the global minimum of the cost function and finally outputs the optimal path point sequence.

[0011] In the above solution, in step S3, the update methods of particle velocity and position are as follows: ; where is the velocity of particle at time , is the velocity of particle at time , is the position of particle at time , is the position of particle at time , is the inertia coefficient; , are the individual and global acceleration coefficients respectively; , is for Two random values within a range.

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

[0013] In the above solution, the specific method of step S4 is as follows: The multiple shooting technique is used to transform the continuous-time optimal control problem into a discrete optimization problem. The explicit Euler method is used to perform numerical integration with the sampling period to obtain the discrete prediction model , , , is the state, control input, and disturbance estimate at time and prediction horizon within, the following nonlinear programming problem is constructed, that is, minimizing the cost function: ; where , respectively represent the sets of states and control inputs within the prediction horizon; The first term is the trajectory tracking term, defined as follows: ; where , , , respectively represent the position, velocity, attitude angle, and attitude angular velocity of the quadrotor at time , represents the swing angle and swing angular velocity of the payload; is the reference trajectory at time , the superscript denotes "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 symbol denotes the weighted squared norm of the vector with respect to the matrix ; The second term is the control smoothing term, as follows: ; wherein, is the control output of the quadrotor at time ; is the desired control quantity at time , and the superscript represents "reference", defined as the control input in the hovering state; is the control quantity of the quadrotor at time ; , are the weight matrices for the control quantity and control smoothness respectively; The third term is the active obstacle avoidance term, including the quadrotor obstacle avoidance cost and the payload obstacle avoidance cost: ; wherein, is the set of obstacles within the detection range of the quadrotor sensor; and are the smoothness parameters for the quadrotor and the payload; is the Euclidean distance from the quadrotor to the obstacle , is the Euclidean distance from the payload to the obstacle ; and are the obstacle avoidance radii of the quadrotor and the payload respectively; The fourth term is the terminal cost function, and its weight matrix is , and the subscript is uniformly represented by " ", satisfying , ensuring the convergence of the state at the end of the prediction horizon.

[0014] In the above solution, the system constraints in step S4 are as follows: ; ; ; ; wherein, is the initial condition, , respectively represent the feasible regions of the system state and the control input, and are expressed as follows: ; ; represents dimensional real number space, is the maximum speed, The upper limit of the thrust provided for each propeller.

[0015] In the above solution, in step S4, it is solved through the sequential quadratic programming framework. Among them, the Gauss-Newton method is used to transform the non-linear programming problem into a series of quadratic optimization problem sub-problems, and the qpOASES solver is used for calculation. At the same time, the real-time iteration strategy and the warm start technology are combined to ensure the real-time performance of the calculation; finally, the ACADO toolchain is used to realize the automated process from modeling to code generation, and complete the high-precision trajectory tracking control in a complex disturbance environment.

[0016] Through the above technical solution, an optimal trajectory planning method for a quadrotor suspended payload system provided by the present invention has the following beneficial effects: 1. The present invention adopts an energy-based Euler-Lagrange modeling method, and realizes the collaborative optimization of load swing suppression and dynamic obstacle avoidance through the system coupling dynamics characteristics, providing an efficient and reliable solution for application scenarios such as low-altitude logistics transportation; 2. The present invention combines a predefined-time disturbance observer with a non-linear model predictive control framework, significantly improving the anti-interference ability of the system. Theoretical proof shows that the predefined-time disturbance observer can estimate the lumped disturbance within a preset time and there is no steady-state error. Using the compensated model for the prediction optimization of non-linear model predictive control effectively guarantees the control accuracy; 3. The present invention proposes a two-stage optimal trajectory planning strategy based on dynamic window particle swarm optimization and non-linear model predictive control. First, the dynamic window particle swarm optimization algorithm is used to generate a local reference trajectory that meets the requirements within the local perception range, and then the non-linear model predictive control is used to dynamically optimize the trajectory. Finally, the real-time optimal trajectory planning of the quadrotor suspended payload system in a complex environment is realized. Brief Description of the Drawings

[0017] In order to more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the drawings required for the description of the embodiments or the prior art.

[0018] Figure 1 A schematic diagram of a quadrotor suspended payload system disclosed in an embodiment of the present invention; Figure 2 A structural diagram of the overall control system of a quadrotor suspended payload system disclosed in an embodiment of the present invention; Figure 3 A schematic diagram of an obstacle avoidance cost function disclosed in an embodiment of the present invention; Figure 4 A schematic diagram of an equidistant planning cost function disclosed in an embodiment of the present invention; Figure 5Schematic diagram of a smooth path cost function disclosed in an embodiment of the present invention; Figure 6 Schematic diagram of a minimum path length cost function disclosed in an embodiment of the present invention; Figure 7 Schematic diagram of an adaptive planning cost function disclosed in an embodiment of the present invention; Figure 8 Schematic diagram of a smooth climbing cost function disclosed in an embodiment of the present invention; Figure 9 Schematic diagram of a two-stage optimal trajectory planning disclosed in an embodiment of the present invention. Detailed implementation manners

[0019] Next, the technical solutions in the embodiments of the present invention will be clearly and completely described with reference to the accompanying drawings in the embodiments of the present invention.

[0020] The present invention provides an optimal trajectory planning method for a quadrotor suspended payload system, which realizes stable control and trajectory planning in a complex environment through predefined time disturbance observer disturbance compensation and dynamic window particle swarm optimization trajectory optimization. Its innovative non-linear model predictive control framework supports real-time obstacle avoidance and strong wind resistance operations, significantly improving the transportation efficiency compared to manual operation, and can be widely applied to high-precision hoisting scenarios such as power inspection and high-altitude rescue, providing a safe and reliable solution for intelligent hoisting of unmanned aerial vehicles.

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

[0022] The schematic diagram of the quadrotor suspended payload system is as Figure 1 shown; among them, the shape of the quadrotor adopts an "X" configuration; for the convenience of describing the motion state of the system, an inertial coordinate system is defined as the global reference system, which always remains stationary; represents the body coordinate system, which is fixedly connected to the quadrotor fuselage and moves with the quadrotor. Its origin coincides with the center of mass of the quadrotor; in addition, a payload coordinate system is also established, and its origin coincides with the center of mass of the quadrotor, and its direction is always parallel to the inertial coordinate system ; next, the six degrees of freedom of the quadrotor can be defined in the inertial coordinate system as and , respectively representing its position and attitude vectors; define as the swing angle vector of the payload in the coordinate system , which is equivalent to the swing angle vector in the inertial coordinate system , and is caused by the payload rotating around and formed by counterclockwise rotation; The quadrotor suspended payload system has 8 degrees of freedom. Select the generalized coordinates as , and according to the Euler-Lagrange equation, the dynamic equation of the system is expressed as follows: ; where, , respectively represent the first and second derivatives of; is the inertia matrix, represents the Coriolis and centrifugal matrix, is the gravity vector; is the control allocation matrix, is the control matrix, where , representing the thrust generated by the th propeller, , represents the aerodynamic coefficient, is the th propeller speed; is the air resistance, is the air resistance coefficient matrix, is the lumped disturbance vector containing unmodeled dynamics and wind disturbances, which is regarded as the external disturbance of the system.

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

[0024] The overall structure diagram of the system is as shown in Figure 2 ; First, obtain the local reference trajectory through the trajectory planning module based on the dynamic window particle swarm optimization algorithm, then further optimize it through the nonlinear model predictive control module, and finally input the optimal control quantity into the quadrotor suspended payload system to obtain the optimal trajectory that meets the system constraints and obstacle avoidance requirements; In order to enhance the anti-interference ability of the system, 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 of the nonlinear model predictive control, ensuring that the system can maintain good performance and stability in the face of external disturbances and uncertainties; First, rewrite the dynamic model of the quadrotor suspended payload system into the following expression: ; In the formula, is the measurable state of the system, is the first derivative of, is the control input of the system,​​​​​​​​​​​​​​​​​​​ is a diagonal matrix, is an unknown lumped disturbance acting on the system; and is a non - linear smooth function with respect to the state ; Then, a new state variable and an auxiliary dynamic system are introduced as follows: ; where, is the first derivative of , represents the difference between two state variables, is the coefficient matrix; Finally, the predefined - time non - linear disturbance observer is designed as follows: ; where, is the estimation of , and are the first derivatives of and respectively; is the Lyapunov function, ; is the estimation of ; is a constant, is the predefined time; Through the stability analysis based on the Lyapunov function, it can be known that the observation error will converge to 0 within the predefined time ; Finally, the system state vector is defined as , , , represent the first derivatives of , and respectively; The control vector is re - defined, and the predefined - time non - linear disturbance observer is used to estimate the lumped disturbance online, and the compensated nominal model obtained is as follows: ; where, represents the first derivative of , represents the estimation of , represents the non - linear function describing the system dynamics.

[0025] S3. Trajectory Planning: Based on the real-time position of the quadrotor, within the detection range of the sensors it carries, the dynamic window particle swarm optimization algorithm is used to minimize the cost function with respect to the path points to find the initial optimal trajectory.

[0026] The path planning problem is formulated through a linear combination of cost functions that incorporate the optimal trajectory criteria and the quadrotor obstacle avoidance constraints. Subsequently, the dynamic window particle swarm optimization algorithm is used to minimize the total cost to find the optimal trajectory. It is worth emphasizing that to ensure the smoothness of the reference trajectory of the trajectory, a cubic spline curve is used to generate a smooth trajectory passing through all path points. 1. Obstacle Avoidance: As Figure 3 shown, let the obstacle avoidance radius of each particle be , and the position of the newly solved path point be ; the center coordinates of the smallest cylinder surrounding the obstacle are , and the radius is . The desired safety distance is , where .

[0027] 2. Equi-distance Planning: To reach the target point with as few planning times as possible, it is necessary to define a lower limit for the distance between adjacent two path points; at the same time, to avoid the distance between adjacent two path points being too large and crossing obstacles, resulting in the loss of the obstacle avoidance effect, it is necessary to define an upper limit; in short, it is necessary to make the path points uniform to achieve a reasonable distribution; it is stipulated that the distance range between adjacent path points is , , where , the distance parameter satisfies ; let the position of the current path point be , define , where and represent the two-dimensional plane components of and . The relevant cost function is defined as: ; As Figure 4 shown, assume , and are the positions of three candidate particles, and only meets the planned distance requirements, so it is used as the new path point.

[0028] 3. Smooth Path: To suppress the swing of the payload, it is required that the reference path be as smooth as possible; represent the previous path point as , it is defined that is the vector pointing from to , and is the vector pointing from to ; then, the angle between the two vectors determines the smoothness of the path, which is expressed as follows: ; where ; as shown in Figure 5 , assuming that and are two possible positions of the new path point, and the corresponding smoothness costs are and , if there exists , then the next path point is preferentially selected as .

[0029] 4. Minimize the path length: To improve the task execution efficiency, it is necessary to shorten the path length as much as possible; in the case of an unknown environment, moving along the straight line between the starting point and the target point as much as possible usually results in a shorter path and can avoid the situation of getting farther and farther away; assuming that the starting point and the ending point coordinates are and , minimizing the and angle between can ensure that the new path point is always as close as possible to the straight line , and the relevant cost function is expressed as follows: ; As shown in Figure 6 , since , so is selected as the new path point.

[0030] 5. Adaptive planning: In the actual environment, there are often various external interferences, which cause 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 so that even if the quadrotor deviates from the current trajectory, it can be re-planned to continue guiding the quadrotor to move towards the target point; assuming that the current position of the quadrotor is , the adaptive planning aims to generate the path point closest to the straight line , thereby expanding the attraction domain of the system and enhancing the robustness; the adaptive planning can be achieved by minimizing the angle between the vector and : ; As shown in Figure 7 , according to , As a new path point.

[0031] 6. Smooth climb: To quickly lift the payload, the trajectory in the initial stage needs to rise rapidly. Additionally, considering the requirement for trajectory smoothness, the vertical trajectory is finally selected to be in the shape of a quadratic curve; define , where and represent as well as the vertical components of ; The function is defined as follows: ; where the angle is the angle between and ; where the symbol " " represents a general placeholder; as Figure 8 shown, the curve segment between and represents the projection of the reference trajectory on the two-dimensional plane. The planned part is shown as a solid line, and the unplanned part is shown as a dashed line; the quadratic curve between and represents the fitting curve of the reference trajectory in the vertical direction, taking into account both smoothness and climb requirements; The total cost function is the weighted sum of all cost functions and is expressed as follows: ; where represents the number of obstacles within the sensor detection range, and is the weight coefficient.

[0032] 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: (1) Cost evaluation: Each particle calculates the path cost based on its current position, and its quality is quantified by the predefined cost function ; (2) Individual and group cooperation: Each particle records its historical optimal position , that is, the path point with the minimum cost in its own exploration. By comparing the costs of all particles, the group can share the global optimal position , that is, the path point with the lowest cost found among all current particles; (3) Position update: The particle dynamically adjusts its velocity and position by combining and information to balance local exploration and global convergence; (4) Iterative optimization: Repeat the above processes (1)-(3). The particle swarm gradually approaches the global minimum of the cost function and finally outputs the optimal path point sequence.

[0033] The update methods of the particle velocity and position are as follows: ; where is the velocity of particle at time , is the velocity of particle at time , is the position of particle at time , is the position of particle at time , is the inertia coefficient; , are the individual and global acceleration coefficients respectively; , are two random values within the range.

[0034] Traditional particle swarm optimization algorithms usually need to input the information of the entire map in the initialization stage. Although this can obtain better search effects, it depends on global information and is not applicable to unknown environments; in actual situations, the detection range of the sensors on the quadrotor is limited, that is, usually only the map information of a certain area around the quadrotor can be obtained. Therefore, traditional particle swarm optimization algorithms will no longer be applicable; by abstracting the sensor detection range as a window that moves with the quadrotor and restricting the search space of the particle swarm within it, the particle swarm optimization algorithm will only search in the currently known area, thus realizing local particle swarm optimization, that is, the dynamic window particle swarm optimization algorithm. As Figure 9 shown, the quadrotor only detects the obstacles existing within the window and conducts local path planning; the black dots in the figure represent the optimal particle positions, 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.

[0035] S4. Optimal control: Use the compensated nominal model as the new prediction model, take the initial optimal trajectory as the reference trajectory, and use nonlinear model predictive control to minimize the cost function, thereby obtaining the prediction trajectory that satisfies the system constraints.

[0036] The continuous-time optimal control problem is transformed into a discrete optimization problem by using the multiple shooting technique, and the explicit Euler method is used for numerical integration with the sampling period to obtain a discrete prediction model , , , where are the state, control input, and disturbance estimate at time and within the prediction time domain , the following nonlinear programming problem is constructed: ; The system constraints are as follows: ; ; ; ; where , respectively represent the sets of states and control inputs within the prediction domain, is the initial condition, , respectively represent the feasible domains of the system state and control input, which are expressed as follows: ; ; denotes the -dimensional real number space, is the maximum speed, is the upper limit of the thrust provided by each propeller; The first term is the trajectory tracking term, which is defined as follows: ; where , , , respectively represent the position, velocity, attitude angle, and attitude angular velocity of the quadrotor at time , represent the swing angle and swing angular velocity of the payload; is the reference trajectory at time , and the superscript denotes "reference" and is generated by the trajectory planning module; , , , , and are both weight matrices, and their subscripts are used to distinguish different matrices; the symbol represents the weighted squared norm of the vector with respect to the matrix ; The second term is the control smoothing term, as shown below: ; where is the control input of the quadrotor at time ; is the desired control quantity at time , and the superscript represents "reference", which is defined as the control input in the hover state; is the control quantity of the quadrotor at time ; , is a weight matrix, and its subscript is used to distinguish different matrices; The third term is the active obstacle avoidance term, which includes the quadrotor obstacle avoidance cost and the payload obstacle avoidance cost: ; where is the set of obstacles within the detection range of the quadrotor sensor; , are the smoothness parameters for the quadrotor and the payload, respectively; is the Euclidean distance from the quadrotor to the obstacle , is the Euclidean distance from the payload to the obstacle ; , are the obstacle avoidance radii of the quadrotor and the payload, respectively; The fourth term is the terminal cost function, and its weight matrix is , and the subscript is uniformly represented by " ", satisfying to ensure the convergence of the state at the end of the prediction horizon.

[0037] The above nonlinear programming problem is solved through the sequential quadratic programming framework, in which the Gaussian-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 the warm start technology are combined to ensure the real-time performance of the calculation; finally, the ACADO toolchain is used to realize the automated process from modeling to code generation, and complete the high-precision trajectory tracking control in a complex disturbance environment.

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

[0039] The above description of the disclosed embodiments enables those skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to those skilled in the art, and the general principles defined herein can be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the broadest scope consistent with the principles and novel features disclosed herein.

Claims

1. An optimal trajectory planning method for a quadrotor suspended payload system, characterized in that It includes the following steps: S1. System Modeling: Establish a dynamic model of the quadrotor suspended payload system; S2. Disturbance Compensation: For the unknown lumped disturbances in the established model, estimate the lumped disturbances through a predefined-time nonlinear disturbance observer to obtain a compensated nominal model; S3. Trajectory Planning: Based on the real-time position of the quadrotor, within the detection range of the sensors carried by it, use the dynamic window particle swarm optimization algorithm to minimize the cost function with respect to the path points and find the initial optimal trajectory; S4. Optimal Control: Take the compensated nominal model as the new prediction model, take the initial optimal trajectory as the reference trajectory, and use nonlinear model predictive control to minimize the cost function to obtain a prediction trajectory that satisfies the system constraints.

2. The optimal trajectory planning method for a quadrotor suspended payload system according to claim 1, characterized in that In step S1, the Euler-Lagrange modeling method is adopted to establish the quadrotor suspended payload system model as follows: ； wherein, are the generalized coordinates, including the position and attitude of the quadrotor, as well as the swing angle of the payload; , respectively represent the first and second derivatives of ; is the inertia matrix, represents the Coriolis and centrifugal matrix, is the gravity vector; is the control allocation matrix, is the control matrix, where , represents the thrust generated by the -th propeller, represents the aerodynamic coefficient, is the -th propeller speed; is the air resistance, is the air resistance coefficient matrix, is the lumped disturbance vector including the unmodeled dynamics and wind disturbances, which is regarded as the external disturbance of the system.

3. The optimal trajectory planning method for a quadrotor suspended payload system according to claim 2, wherein The specific method of step S2 is as follows: First, rewrite the dynamic model of the quadrotor suspended payload system into the following expression: ； wherein, is the measurable state of the system, is 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 are non-linear smooth functions with respect to the state ; Then, a new state variable is introduced and the auxiliary dynamic system is as follows: ； Among them, is the first derivative of, represents the difference between two state variables, is the coefficient matrix; Finally, design the predefined-time nonlinear disturbance observer as follows: ； Among them, is the estimation of , and are the first-order derivatives of and respectively; is the Lyapunov function, ; is the estimation of ; is a constant, is a predefined time; It can be seen from the stability analysis based on the Lyapunov function that the observation error will converge to 0 within a predefined time ; Finally, define the system state vector as , , , which represent respectively , and the first-order derivatives; redefine the control vector , and use a predefined time nonlinear disturbance observer to estimate the lumped disturbance online. The compensated nominal model obtained is as follows: ； Among them, denotes the first derivative of denotes the estimate of denotes the nonlinear function that describes the system dynamics.

4. The optimal trajectory planning method for a quadrotor suspended payload system according to claim 1, wherein In step S3, the cost function with respect to the path points is as follows: ； Among them, , , , , , respectively represent the cost functions for obstacle avoidance, equidistant planning, smooth path, minimizing path length, adaptive planning, and smooth climbing; is the weight corresponding to the above cost function; represents the number of obstacles within the detection range of the sensor.

5. The optimal trajectory planning method for a quadrotor suspended payload system according to claim 4, characterized in that 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: (1) Cost evaluation: Each particle calculates the path cost based on its current position, and quantifies its advantages and disadvantages by a predefined cost function ; (2) Individual and group collaboration: Each particle records its historical optimal position , which is the path point with the minimum cost in its own exploration. By comparing the costs of all particles, the group can find the globally optimal position shared by the group , that is, the path point with the lowest cost found among all current particles; (3) Position update: The particle dynamically adjusts its velocity and position by combining and information to balance local exploration and global convergence; (4) Iterative Optimization: Repeat the above processes (1)-(3), and the particle swarm gradually approaches the global minimum of the cost function, and finally outputs the optimal path point sequence.

6. The optimal trajectory planning method for a quadrotor suspended payload system according to claim 5, characterized in that In step S3, the update methods of the particle velocity and position are as follows: ； Among them, is the velocity of the particle at time . is the velocity of the particle at time . is the position of the particle at time . is the position of the particle at time . is the inertia coefficient; , are the individual and global acceleration coefficients respectively; , are two random values within the range.

7. The optimal trajectory planning method for a quadrotor suspended payload system according to claim 1, characterized in that In step S3, limit the search space of the particle swarm within the detection range of the sensors, so as to realize local particle swarm optimization, that is, the dynamic window particle swarm optimization algorithm.

8. The optimal trajectory planning method for a quadrotor suspended payload system according to claim 1, characterized in that, The specific method of step S4 is as follows: The continuous-time optimal control problem is transformed into a discrete optimization problem by using the multiple shooting technique, and the explicit Euler method is used for numerical integration with the sampling period to obtain a discrete prediction model , , , where are the state, control input, and disturbance estimate at time ; within the time and the prediction horizon, the following nonlinear programming problem is constructed to minimize the cost function: ； Among them, , respectively represent the sets of states and control variables within the prediction horizon; The first item is the trajectory tracking item, defined as follows: ； Among them, , , , respectively represent the position, velocity, attitude angle, and attitude angular velocity of the quadrotor at a certain moment; , represent the swing angle and swing angular velocity of the payload; is the reference trajectory at time , and the 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 symbol represents the weighted squared norm of the vector with respect to the matrix ; The second item For the control smoothing term, as follows: ； Among them, is the control output of the quadrotor at time ; is the desired control quantity at time . The superscript represents "reference", which is defined as the control input in the hovering state; is the control quantity of the quadrotor at time ; , are the weight matrices for the control quantity and control smoothness respectively; The third item is the active obstacle avoidance item, including the obstacle avoidance cost of the quadrotor and the obstacle avoidance cost of the payload: ； Among them, is the set of obstacles within the detection range of the quadrotor sensor; and are the smoothness parameters regarding the quadrotor and the payload; is the Euclidean distance from the quadrotor to the obstacle ; is the Euclidean distance from the payload to the obstacle ; and are the obstacle avoidance radii of the quadrotor and the payload respectively; The fourth item is the terminal cost function, and its weight matrix is , using " " to uniformly represent the subscripts, satisfying , ensuring the convergence of the state at the end of the prediction horizon.

9. The optimal trajectory planning method for a quadrotor suspended payload system according to claim 8, characterized in that, The system constraints in step S4 are as follows: ； ； ； ； wherein, is the initial condition, , respectively represent the feasible regions of the system state and the control input, and are expressed as follows: ； ； denote an n-dimensional real space is the maximum speed is the upper limit of the thrust provided by each propeller 10. The optimal trajectory planning method for a quadrotor suspended payload system according to claim 1, characterized in that In step S4, solve it through the sequential quadratic programming framework, where the Gauss-Newton method is used to transform the nonlinear programming problem into a series of quadratic optimization problem sub-problems, and the qpOASES solver is used for calculation. At the same time, combine the real-time iteration strategy and the warm start technology to ensure the real-time performance of the calculation; finally, use the ACADO toolchain to realize the automated process from modeling to code generation, and complete the high-precision trajectory tracking control in a complex disturbance environment.

Images