Unmanned aerial vehicle trajectory tracking method and system based on MPC-ESO and path optimization

Abstract

The invention discloses an unmanned aerial vehicle trajectory tracking method and system based on MPC-ESO and path optimization, relates to the technical field of unmanned aerial vehicle control, and is used for solving the problems of weak anti-interference capability and response lag in the unmanned aerial vehicle trajectory tracking process in the prior art. And the real-time and robustness requirements of unmanned aerial vehicle trajectory tracking in a complex scene are difficult to meet. The unmanned aerial vehicle trajectory tracking method based on MPC-ESO and path optimization comprises the following steps: acquiring environment information by using an unmanned aerial vehicle, and establishing a grid map according to the environment information; an algorithm is improved by designing a cost function, a grid map is used as input data of path planning, and a path is planned in real time through the improved algorithm; performing attitude control and height position control on the unmanned aerial vehicle; and according to the planned path, predicting and controlling the trajectory of the unmanned aerial vehicle by using a model, and performing feedforward compensation on the unmanned aerial vehicle control quantity through an extended state observer to realize trajectory tracking of the unmanned aerial vehicle.

Description

Technical Field

[0001] The present invention relates to the field of unmanned aerial vehicle (UAV) control technology, and more specifically, to a UAV trajectory tracking method and system based on MPC-ESO and path optimization. Background Art

[0002] With the development of technology, drones have been widely used in military reconnaissance, logistics distribution, agricultural monitoring and other fields due to their flexibility, high efficiency and low cost, and the mission environment has gradually become more dynamic and requires high precision.

[0003] The Model Predictive Control (MPC) algorithm has the ability to explicitly handle complex system constraints. It uses a rolling optimization and feedback correction strategy to online correct the predicted trajectory at each moment and has strong anti-interference ability. The main advantage of the MPC algorithm is that it can predict the near future moment under the premise of meeting certain constraints, and when making such predictions, it will comprehensively consider the predictions of all future moments in the prediction time domain. However, the existing MPC control has insufficient anti-interference ability, weak adaptability to dynamic environments, and high computational complexity, which cannot meet the control requirements. Although the existing disturbance observer DOB can estimate disturbances, its structure is complex and difficult to coordinate with the MPC controller. Due to the above-mentioned defects in the existing technology, the UAV trajectory tracking process has weak anti-interference ability and delayed response, which makes it difficult to meet the real-time and robustness requirements of UAV trajectory tracking in complex scenarios. Summary of the Invention

[0004] The purpose of the present invention is to provide a UAV trajectory tracking method and system based on MPC-ESO and path optimization. This method addresses the technical issues that the existing UAV trajectory tracking technology has, such as weak anti-interference capabilities and delayed response, and its inability to meet the real-time and robustness requirements of UAV trajectory tracking in complex scenarios. In view of this, the present invention is implemented through the following solution.

[0005] In a first aspect, the present invention provides a UAV trajectory tracking method based on MPC-ESO and path optimization, comprising: Using drones to acquire environmental information and creating a grid map based on the environmental information; Improvement through design of cost function algorithm, and uses the grid map as input data for path planning, and improves the The algorithm plans the path in real time; Perform attitude control and altitude position control on the UAV; According to the planned path, the UAV trajectory is controlled by using the model prediction, and the UAV control variable is feedforward compensated through the extended state observer to achieve UAV trajectory tracking.

[0006] Compared with the prior art, the UAV trajectory tracking method based on MPC-ESO and path optimization of the present invention uses the UAV to obtain environmental information and then establishes a grid map based on the environmental information; After the algorithm is improved, the grid map is used as the input data for path planning. The algorithm plans the path in real time; further, after the attitude control and altitude position control of the UAV are performed, the trajectory of the UAV is controlled by the model prediction according to the above-mentioned planned path, and the UAV control quantity is feedforward compensated by the extended state observer, thereby realizing the trajectory tracking of the UAV. The algorithm is improved to increase the efficiency and effectiveness of path planning. After the model predicts and controls the drone's trajectory, the drone's control variable is further feedforward compensated through an extended state observer, improving the anti-interference and responsiveness of the drone's trajectory tracking. Furthermore, the combination of the above-mentioned real-time path planning and model predictive control enhances the drone's trajectory tracking's ability to cope with complex dynamic environments, effectively achieving real-time obstacle avoidance and improving the safety of drone flight. The above-mentioned technical solution of the present invention solves the technical problem that the existing technology has weak anti-interference ability and delayed response in the drone trajectory tracking process, making it difficult to meet the real-time and robustness requirements of drone trajectory tracking in complex scenarios.

[0007] Furthermore, in the UAV trajectory tracking method based on MPC-ESO and path optimization of the present invention, the improved The algorithm plans the path in real time, including: Design path curvature penalty term; Design the cost function using the path curvature penalty term; Create an open list and a closed list, and add the starting point to the open list; Take the node with the smallest cost function value from the open list. If the node is the end point, start from the end point and trace back to the starting point through the parent node pointer to generate a path point sequence.

[0008] Furthermore, in the UAV trajectory tracking method based on MPC-ESO and path optimization of the present invention, after taking out the node with the smallest cost function value from the open list, if the open list is empty, returning no solution and traversing the adjacent nodes of the node; Get the cost function value of the adjacent node. If the adjacent node is not in the open list, or the actual cost of the adjacent node is smaller than that of the original node, then update the adjacent node to the parent node, calculate the heuristic function, and insert it into the open list. Repeat until the end is found or the open list is empty and has no solution; If there is a solution, start from the end point and trace back to the starting point through the parent node pointer to generate a sequence of path points.

[0009] Furthermore, in the UAV trajectory tracking method based on MPC-ESO and path optimization of the present invention, in the process of designing the cost function using the path curvature penalty term, the cost function is expressed as: ;in, represents the cost function, Indicates the starting point to the current node The actual cost, represents the heuristic function, represents the smoothing weight coefficient, represents the path curvature penalty term.

[0010] Furthermore, in the UAV trajectory tracking method based on MPC-ESO and path optimization of the present invention, the designed path curvature penalty term includes: Set the current node to n, the predecessor node to n-1, and the successor node to n+1; the coordinates of the current node, predecessor node, and successor node are 、 and ; Defining a vector , ,in, Indicates coordinate points Point to coordinate point The direction vector, Indicates coordinate points Point to coordinate point direction vector; Get the angle between two vectors , according to the angle Get the penalty at the current node n; The penalty at the current node n is expressed as: , represents the smoothing weight coefficient.

[0011] Furthermore, in the UAV trajectory tracking method based on MPC-ESO and path optimization of the present invention, the use of model prediction to control the UAV trajectory includes: Define state quantities and input quantities, and establish a kinematic model; Expand the prediction model and construct the state and input sequence within the prediction time domain N; Formulate an optimization problem and establish an objective function to minimize tracking error and control input penalty; The objective function is converted into a quadratic programming form, and the quadratic programming form solver is called to solve the optimal input sequence online. The control variable is applied as the UAV speed control instruction and then solved again in the next step.

[0012] Furthermore, in the UAV trajectory tracking method based on MPC-ESO and path optimization of the present invention, the feedforward compensation of the UAV control variable by the extended state observer includes: The definition of state vector includes the original state vector and disturbance; The disturbance dynamics is modeled as a first-order inertial process, and the discretized state expansion equation is established; Construct a Lumberg observer based on the discretized state expansion equation and estimate the disturbance; The disturbance estimated by the observer is feedforward compensated to the input of the model predictive control and output as the UAV speed control command.

[0013] Furthermore, in the UAV trajectory tracking method based on MPC-ESO and path optimization of the present invention, the defined state vector includes the original state vector and the disturbance, which is expressed as: ;in, represents the expanded state matrix, represents the coordinate position of the drone on the x-axis at time k, represents the coordinate position of the drone on the y-axis at time k, represents the coordinate position of the drone on the z-axis at time k, represents the external disturbance to the x-axis position of the drone at time k, represents the external disturbance to the y-axis position of the UAV at time k, represents the external disturbance to the UAV’s z-axis position at time k, Represents the transpose of a matrix; And / or, model the disturbance dynamics as a first-order inertial process expressed as: ;in, , represents the external disturbance of the UAV on the three spatial coordinate axes x, y, and z, represents the estimated value of the total external disturbance acting on the i-axis at time k+1, represents the estimated value of the total external disturbance acting on axis i at time k, It represents the disturbance increment acting on the i-axis during the period from time k to time k+1.

[0014] Furthermore, in the UAV trajectory tracking method based on MPC-ESO and path optimization of the present invention, the discretized state expansion equation is expressed as: ;in, represents the expanded state vector at time k+1, represents the expanded state vector at time k, represents the input control quantity at time k, represents the external interference vector at time k, represents the state transition matrix, represents the output matrix, represents the interference matrix, and , , , represents the identity matrix, represents the product of the time step and the identity matrix I, represents a matrix; And / or, the constructed Lumberg observer is expressed as: ;in, represents the estimated expanded state vector at time k+1, represents the estimated expanded state vector at time k, represents the observer gain matrix, represents the measurement output matrix at time k, represents the output matrix, and .

[0015] In a second aspect, the present invention provides a UAV trajectory tracking system based on MPC-ESO and path optimization, comprising: A grid map building module is used to obtain environmental information using a drone and build a grid map based on the environmental information; Planning path module, used to improve the cost function by designing algorithm, and uses the grid map as input data for path planning, and improves the The algorithm plans the path in real time; UAV control module, used to control the attitude and altitude of the UAV; The trajectory tracking module is used to control the UAV trajectory according to the planned path using the model prediction and to perform feedforward compensation on the UAV control variable through the extended state observer to achieve UAV trajectory tracking.

[0016] Compared with the prior art, the beneficial effects of the UAV trajectory tracking system based on MPC-ESO and path optimization of the present invention are the same as the beneficial effects of the UAV trajectory tracking method based on MPC-ESO and path optimization described in the above technical solution, and will not be repeated here. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] The drawings described herein are used to provide a further understanding of the present invention and constitute a part of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the drawings: Figure 1 This is a block diagram of the implementation of the UAV trajectory tracking method based on MPC-ESO and path optimization of the present invention. DETAILED DESCRIPTION

[0018] In order to make the technical problems, technical solutions and beneficial effects to be solved by the present invention more clearly understood, the present invention is further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0019] It should be noted that when an element is referred to as being “fixed on” or “disposed on” another element, it may be directly on the other element or indirectly on the other element. When an element is referred to as being “connected to” another element, it may be directly connected to the other element or indirectly connected to the other element.

[0020] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be understood to indicate or imply relative importance or implicitly specify the number of the technical features indicated. Thus, a feature specified as "first" or "second" may explicitly or implicitly include one or more of the features. In the description of the present invention, "plurality" means two or more, unless otherwise specifically defined. "Several" means one or more, unless otherwise specifically defined.

[0021] In the existing technology, the Model Predictive Control (MPC) algorithm has the ability to explicitly handle complex system constraints, adopts a rolling optimization and feedback correction strategy, and online corrects the predicted trajectory at each moment, and has strong anti-interference ability. The main advantage of the MPC algorithm is that it can predict the near future moment under the premise of meeting certain constraints, and when making such predictions, it will comprehensively consider the predictions of all future moments in the prediction time domain. However, the existing MPC control has insufficient anti-interference ability, weak adaptability to dynamic environments, and high computational complexity, which cannot meet the control requirements. Although the existing disturbance observer DOB can estimate disturbances, its structure is complex and difficult to coordinate with the MPC controller. Due to the above-mentioned defects in the existing technology, the UAV trajectory tracking process has technical problems such as weak anti-interference ability and delayed response, which makes it difficult to meet the real-time and robustness requirements of UAV trajectory tracking in complex scenarios.

[0022] In order to solve the above technical problems, the present invention provides a UAV trajectory tracking method based on MPC-ESO and path optimization, comprising: Using drones to acquire environmental information and creating a grid map based on the environmental information; Improvement through design of cost function algorithm, and uses the grid map as input data for path planning, and improves the The algorithm plans the path in real time; Perform attitude control and altitude position control on the UAV; According to the planned path, the UAV trajectory is controlled by using the model prediction, and the UAV control variable is feedforward compensated through the extended state observer to achieve UAV trajectory tracking.

[0023] In the case of adopting the above technical solution, in the UAV trajectory tracking method based on MPC-ESO and path optimization of the present invention, after obtaining environmental information by using the UAV, a grid map is established according to the environmental information; After the algorithm is improved, the grid map is used as the input data for path planning. The algorithm plans the path in real time; further, after the attitude control and altitude position control of the UAV are performed, the trajectory of the UAV is controlled by the model prediction according to the above-mentioned planned path, and the UAV control quantity is feedforward compensated by the extended state observer, thereby realizing the trajectory tracking of the UAV. The algorithm is improved to increase the efficiency and effectiveness of path planning. After the model predicts and controls the drone's trajectory, the drone's control variable is further feedforward compensated through an extended state observer, improving the anti-interference and responsiveness of the drone's trajectory tracking. Furthermore, the combination of the above-mentioned real-time path planning and model predictive control enhances the drone's trajectory tracking's ability to cope with complex dynamic environments, effectively achieving real-time obstacle avoidance and improving the safety of drone flight. The above-mentioned technical solution of the present invention solves the technical problem that the existing technology has weak anti-interference ability and delayed response in the drone trajectory tracking process, making it difficult to meet the real-time and robustness requirements of drone trajectory tracking in complex scenarios.

[0024] In order to better understand the present invention, the content of the present invention is further explained below in conjunction with specific examples, but the content of the present invention is not limited to the following examples.

[0025] Example 1 This embodiment provides a UAV trajectory tracking method based on MPC-ESO and path optimization, including: Step 1: Using a drone to obtain environmental information and creating a grid map based on the environmental information; Step 2: Improve by designing cost function algorithm, and uses the grid map as input data for path planning, and improves the The algorithm plans the path in real time; Step 3: Control the attitude and altitude of the drone; Step 4: Based on the planned path, the UAV trajectory is controlled by using the model prediction, and the UAV control variable is feedforward compensated through the extended state observer to achieve UAV trajectory tracking.

[0026] Example 2 This embodiment provides a UAV trajectory tracking method based on MPC-ESO and path optimization, including: S100, using a drone to obtain environmental information, and creating a grid map based on the environmental information; S200, improved by designing cost function algorithm, and uses the grid map as input data for path planning, and improves the The algorithm plans the path in real time; Furthermore, the content of step S200 includes: S201, design path curvature penalty term; S202, designing a cost function using a path curvature penalty term; Furthermore, the cost function is expressed as: ;in, represents the cost function, Indicates the starting point to the current node The actual cost, represents the heuristic function, represents the smoothing weight coefficient, represents the path curvature penalty term; S203, creating an open list and a closed list, and adding the starting point to the open list; S204, taking the node with the smallest cost function value from the open list. If the node is the end point, starting from the end point, trace back to the starting point through the parent node pointer to generate a path point sequence; S205, after taking out the node with the smallest cost function value from the open list, if the open list is empty, return no solution and traverse the adjacent nodes of the node; S206, obtaining the cost function value of the adjacent node. If the adjacent node is not in the open list, or the actual cost of the adjacent node is smaller than that of the original node, the adjacent node is updated as the parent node, the heuristic function is calculated, and the adjacent node is inserted into the open list. S207, repeat until the end is found or the open list is empty and has no solution; S208, if there is a solution, start from the end point and trace back to the starting point through the parent node pointer to generate a path point sequence; Furthermore, the designed path curvature penalty term includes: S2011, set the current node to n, the predecessor node to n-1, and the successor node to n+1; the coordinates of the current node, predecessor node, and successor node are 、 and ; S2012, defining vectors , ,in, Indicates coordinate points Point to coordinate point The direction vector, Indicates coordinate points Point to coordinate point direction vector; S2013, get the angle between vectors , according to the angle Get the penalty at the current node n; The penalty at the current node n is expressed as: , represents the smoothing weight coefficient.

[0027] S300 controls the attitude and altitude of the drone.

[0028] S400, according to the planned path, using the model to predict and control the trajectory of the UAV, and performing feedforward compensation on the UAV control variable through the extended state observer to achieve UAV trajectory tracking; Furthermore, in step S400, the trajectory of the UAV is controlled by using the model prediction, including: S411, define state quantities and input quantities, and establish a kinematic model; S412, performing prediction model expansion to construct the state and input sequence within the prediction time domain N; S413, construct the optimization problem and establish the objective function to minimize the tracking error and control input penalty; S414, converting the objective function into a quadratic programming form, calling a quadratic programming form solver to solve the optimal input sequence online, applying the control variable as the UAV speed control instruction, and resolving in the next step; Furthermore, the UAV control variable is fed-forward compensated through the extended state observer, including: S421, defining a state vector including an original state vector and a disturbance; S422, the disturbance dynamics is modeled as a first-order inertial process, and the discretized state expansion equation is established; S423, constructing a Lumberg observer based on the discretized state expansion equation and estimating the disturbance; S424, feedforward compensating the disturbance estimated by the observer to the input of the model predictive control and outputting it as a speed control command of the UAV; Furthermore, the defined state vector includes the original state vector and the disturbance, which is expressed as: ;in, represents the expanded state matrix, represents the coordinate position of the drone on the x-axis at time k, represents the coordinate position of the drone on the y-axis at time k, represents the coordinate position of the drone on the z-axis at time k, represents the external disturbance to the x-axis position of the drone at time k, represents the external disturbance to the y-axis position of the UAV at time k, represents the external disturbance to the UAV’s z-axis position at time k, Represents the transpose of a matrix; Furthermore, the disturbance dynamics is modeled as a first-order inertial process, expressed as: ;in, , represents the external disturbance of the UAV on the three spatial coordinate axes x, y, and z, represents the estimated value of the total external disturbance acting on the i-axis at time k+1, represents the estimated value of the total external disturbance acting on axis i at time k, It represents the disturbance increment acting on the i-axis during the period from time k to time k+1; Furthermore, the discretized state expansion equation is expressed as: ;in, represents the expanded state vector at time k+1, represents the expanded state vector at time k, represents the input control quantity at time k, represents the external interference vector at time k, represents the state transition matrix, represents the output matrix, represents the interference matrix, and , , , represents the identity matrix, represents the product of the time step and the identity matrix I, represents a matrix; Furthermore, the constructed Lumberg observer is expressed as: ;in, represents the estimated expanded state vector at time k+1, represents the estimated expanded state vector at time k, represents the observer gain matrix, represents the measurement output matrix at time k, represents the output matrix, and .

[0029] Example 3 First, see Figure 1 This embodiment provides a UAV trajectory tracking method based on MPC-ESO and path optimization, including: S100: The drone uses radar or depth cameras to sense environmental information, including the location of dynamic or static obstacles and the drone's position. A grid map is created based on this information. Furthermore, after the grid map is established, the obstacle grid is expanded. The specific formula is: ;in, represents the expansion radius, represents the radius of the drone, Indicates safety margin.

[0030] S200, improved by designing cost function algorithm, and uses the grid map as the input data for path planning. The algorithm plans the path in real time; Furthermore, the content of step S200 includes: S201, design path curvature penalty term; Furthermore, the designed path curvature penalty term includes: S2011, set the current node to n, the predecessor node to n-1, and the successor node to n+1; the coordinates of the current node, predecessor node, and successor node are 、 and ; S2012, defining vectors , ,in, Indicates coordinate points Point to coordinate point The direction vector, Indicates coordinate points Point to coordinate point direction vector; S2013, get the angle between vectors , according to the angle Get the penalty at the current node n; The penalty at the current node n is expressed as: , represents the smoothing weight coefficient; S202, designing a cost function using a path curvature penalty term; Furthermore, the cost function is expressed as: ;in, represents the cost function, Indicates the starting point to the current node The actual cost, represents the heuristic function, represents the smoothing weight coefficient, represents the path curvature penalty term; In the above cost function: ; in, represents the Euclidean distance from the starting point to the current node n, represents the x-coordinate of the starting point, Indicates the x-coordinate of the current node n, represents the y coordinate of the starting point, Indicates the y coordinate of the current node n, represents the z coordinate of the starting point, Indicates the z coordinate of the current node n; ; in, Indicates the heuristic value of the diagonal distance from the current node n to the end point, Indicates taking the maximum value, Indicates the absolute value of the difference between the current node and the end point x coordinate, Indicates the absolute value of the y-coordinate difference between the current node and the end point. Indicates the absolute value of the z-coordinate difference between the current node and the end point. Indicates taking the middle value. Indicates taking the minimum value; S203, establish an open list (Open List) and a closed list (Closed List); the open list uses a priority queue and is sorted by total cost. Sorting; the closed list uses a hash table structure to store processed nodes and adds the starting point to the open list; S204, take out from the open list Smallest node ,like If it is the end point, then backtrack the path. If the open list is empty, return no solution. S205, traverse the nodes in step S204 Due to the high complexity of the neighborhood expansion of the three-dimensional grid map, a hierarchical pruning method is used to avoid redundant calculations, that is, only the nodes near the height of the drone are calculated, and the nodes Axis coordinates The following conditions should be met: ;in, Indicates the current altitude of the drone. Indicates the set search range parameters; S206, calculate the cost function of adjacent nodes Value, if the adjacent node is not in the open list, or the adjacent node is larger than the original node If it is smaller, then the adjacent node is updated as the parent node and the calculation , insert it into the open list; S207, repeat steps S204 to S206 until the end is found or the open list is empty; if there is a solution, proceed to step S208; S208, starting from the end point, tracing back to the starting point through the parent node pointer to generate a path point sequence; S300 uses the method of path point thinning to The algorithm outputs dense path points for key point extraction, such as curvature extreme points and path turning points, to retain geometric features while reducing the number of points; specifically: S301, taking a curve composed of ordered points as input and selecting a threshold ε to control the simplification accuracy, the larger the value, the higher the simplification degree; S302, selecting a starting point P0 and a point Pn to form a straight line segment L; S303: For each intermediate point Pi, calculate its vertical distance to the line segment L using the following formula: ;in, Represents the vertical distance from point Pi to line segment L, represents vector P1, represents the modulus of the vector, Pi represents the i-th point, , ,× represents vector cross product, represents the x-coordinate of point Pi, represents the y coordinate of point Pi, Represents the z coordinate of point Pi; S304, find the maximum value dmax among all di and record the corresponding point Pmax; S305: If dmax > ε, divide the curve into two segments and recursively execute steps S301 to S304 for the left and right ends respectively. If dmax <= ε, discard all the middle segments and only keep the endpoints P0 and Pn. S306: After the recursion is completed, all the retained points form a simplified curve P.

[0031] S400 uses cubic spline interpolation to generate a continuous trajectory to ensure the continuity of path position, velocity, and acceleration, and to ensure kinematic feasibility. The trajectory is represented as follows: ;in, Indicates the x-axis trajectory position, represents the constant term coefficient in the x-direction of the i-th segment, represents the coefficient of the first-order term in the x-direction of the i-th segment, represents the coefficient of the quadratic term in the x-direction of the i-th segment, represents the coefficient of the cubic term in the x-direction of the i-th segment, represents the normalized path length parameter, Indicates the y-axis trajectory position, represents the constant term coefficient in the y direction of the i-th segment, represents the coefficient of the first-order term in the y direction of the i-th segment, represents the coefficient of the quadratic term in the y direction of the i-th segment, represents the coefficient of the cubic term in the y direction of the i-th segment, represents the z-axis trajectory coefficient, represents the constant coefficient of the z direction of the i-th segment, represents the linear coefficient of the z-direction of the i-th segment, represents the coefficient of the quadratic term in the z direction of the i-th segment, represents the coefficient of the cubic term in the z direction of the i-th segment; Furthermore, the above trajectory expression formula wherein the parameter solution method is as follows: S401, solve the coefficient matrix for each dimension independently, Take dimension as an example; S402, define the coefficient vectors of each segment of the polynomial ;in, Represents the transpose of a matrix; S403, based on the continuity conditions and boundary conditions, form the equation group: ;in, represents a tridiagonal matrix containing the derivative constraints of each segment connection point, represents the column vector composed of all segment coefficient vectors, Represents the right side vector consisting of the path point coordinates and the boundary velocity constraint, which is composed of the path point coordinates and the boundary velocity; S404, solving the matrix using the pursuit method; By using cubic spline interpolation, we can eliminate The jagged jitter after the straight lines connect the waypoints makes the UAV motion smoother; the curve formed by the interpolated waypoints is used as the target trajectory of the MPC controller.

[0032] S500, which controls the attitude and altitude of the drone; In this embodiment, the attitude control of the UAV often requires high real-time performance. However, the MPC algorithm has high computational complexity and weak real-time performance. In order to improve computational efficiency and reduce the dimension of MPC trajectory control, the attitude and speed control of the UAV uses the PID algorithm. The attitude controller is the basis for the UAV to achieve precise position control. If the UAV only controls the speed, after reaching the desired position, if the angular velocity is not 0, the position cannot be expected to be stable. Therefore, it adopts a cascade PID design. The outer loop is the angle loop, which usually only uses the P component control, and the inner loop is the angular velocity loop. The specific implementation method of the above-mentioned posture controller includes: S511, angle loop design; the angle loop is responsible for converting the desired angle (such as pitch, roll, and yaw) and the actual angle error into the desired angular velocity. The input is the angular error of the three Euler angles, and the output is the desired angular velocity under the machine system; For angle conversion, angle control is adopted, and the formula is as follows: ; ; in, represents the desired angular velocity vector in the body coordinate system, represents the angle loop proportional gain matrix, represents the roll angular velocity, represents the pitch angular velocity, represents the yaw angular velocity, represents the roll channel proportional gain, Indicates the proportional gain of the pitch channel, represents the proportional gain of the yaw channel, represents the desired roll angle, represents the actual roll angle, represents the desired pitch angle, Indicates the actual pitch angle, represents the desired yaw angle, represents the actual yaw angle, represents a matrix; S512, angular velocity loop design; the angular velocity loop uses the desired angular velocity output by the angle loop as the set value and quickly tracks the actual angular velocity by adjusting the motor pulse width modulation (PWM) signal; the input is the desired angular velocity value, and the output is the desired three-axis torque; the angle loop uses PID control, and the formula is as follows: ;in, represents the desired moment vector of the body coordinate system, represents the angular velocity proportional gain, represents the angular velocity error, represents the angular velocity integral gain, represents the angular velocity differential gain, Indicates the rate of change of angular velocity error; In the UAV control system, (Torque) is quantified as the duty cycle of the PWM wave.

[0033] Furthermore, in the process of UAV altitude position control, the design concept of the position controller is roughly the same as that of the attitude controller, and cascade PID control is also adopted. The outer loop is the position loop and the inner loop is the speed loop. Based on the earth coordinate system, the altitude channel controller in the z-axis direction is designed. Specifically: S521, speed loop design, the implementation method is as follows: The horizontal velocity has the following relationship: ; set up ; The simplified formula is:

[0034] So there is the formula:

[0035] By PID control, the desired velocity derivative (acceleration) can be obtained as follows: ; Based on: ; The desired roll and pitch angles can be obtained; In the above process, represents the horizontal acceleration vector, Indicates the total pulling force of the machine body, Indicates the quality of the drone, represents the actual yaw angle, represents the actual roll angle, Indicates the actual pitch angle, represents the acceleration due to gravity, The rotation matrix representing the yaw angle, represents the attitude angle vector, represents the horizontal attitude angle vector, represents the desired horizontal attitude angle vector, represents the desired horizontal acceleration vector, represents the proportional gain coefficient of horizontal position control, represents the horizontal velocity error, Indicates the integral gain coefficient of horizontal position control, represents the differential gain coefficient of horizontal position control, Indicates the rate of change of horizontal velocity error; S522, height speed controller design, the specific implementation method is as follows: The input of the altitude channel is the expected z-axis speed and the actual z-axis speed, and the output is the expected pulling force of the drone. ; Substitute into the PID algorithm and we get: ; Therefore, the expected tension is: ; In the above formula, represents the z-axis acceleration, represents the acceleration due to gravity, Indicates the actual total pulling force, Indicates the quality of the drone, represents the height proportional gain, Indicates the height error, represents the height integral gain, represents the highly differential gain, represents the rate of change of height error, Indicates the expected total pulling force.

[0036] S600, based on the planned path, using the model to predict and control the trajectory of the UAV, and performing feedforward compensation on the UAV control variable through the extended state observer to achieve UAV trajectory tracking; Furthermore, in step S600, the trajectory of the UAV is controlled by using the model prediction, including: S611, define the state quantity as position x, y and z, define the input quantity as speed , and , and the kinematic model is established as: ;in, represents the state vector at time k+1, represents the state vector at time k, represents the control quantity at time k, represents the state transition matrix, and , represents the input matrix, and , To control the cycle, represents the identity matrix; S612, expand the prediction model and construct the state and input sequence in the prediction time domain N as follows: ; Represent multi-step forecasts using augmented matrices: ; in, Represents the state and input sequence, represents the state at time k+1 predicted at time k, represents the state at time k+N predicted at time k, represents the control quantity at time k, It means the control quantity predicted at time k+N-1 at time k, represents the state vector at time k, and is a block lower triangular matrix constructed recursively from A and B; S613, construct the optimization problem and establish the objective function to minimize the tracking error and control input penalty as: ; in, represents the objective function, The state vector at time k is predicted at time k+i, N represents the prediction length, represents the target state at time k+i, T represents the transpose of the matrix, represents the state error weight matrix, represents the current state vector, represents the current reference trajectory value vector, represents the control quantity at time k+i-1 predicted at time k, represents the input weight matrix; , is the weight matrix, which is set to a diagonal matrix to adjust the tracking accuracy and input smoothness, and determine the constraints , i.e. determining the maximum speed limit; S614, convert the objective function into a quadratic programming form (QP), and the converted objective function is: ; and: ; in, represents the control input sequence vector, T represents the transpose of the matrix, represents the Hessian matrix, represents the linear term coefficient vector, represents the state error weight matrix, represents the input weight vector, represents the state transition matrix, represents the input influence matrix, represents the current state vector, Represents the current reference trajectory sequence vector, and min represents the minimization operation; Call the quadratic programming formalism solver to solve the optimal input sequence online, apply the control variable as the UAV speed control instruction, and re-solve in the next step; It should be noted that using only the MPC controller has the following defects: 1. External disturbances such as wind or load changes will significantly increase the tracking error; 2. Relying solely on feedback regulation, the response speed is insufficient; 3. The existence of unmodeled dynamic quantities leads to insufficient model accuracy and the optimization results deviate from reality.

[0037] Furthermore, the UAV control variable is fed-forward compensated through the ESO extended state observer, including: S621, define the state vector including the original state vector and the disturbance, expressed as: ;in, represents the expanded state matrix, represents the coordinate position of the drone on the x-axis at time k, represents the coordinate position of the drone on the y-axis at time k, represents the coordinate position of the drone on the z-axis at time k, represents the external disturbance to the x-axis position of the drone at time k, represents the external disturbance to the y-axis position of the UAV at time k, represents the external disturbance to the UAV’s z-axis position at time k, Represents the transpose of a matrix; S622, model the disturbance dynamics as a first-order inertial process and establish the discretized state expansion equation; Furthermore, the disturbance dynamics is modeled as a first-order inertial process, expressed as: ;in, , represents the external disturbance of the UAV on the three spatial coordinate axes x, y, and z, represents the estimated value of the total external disturbance acting on the i-axis at time k+1, represents the estimated value of the total external disturbance acting on axis i at time k, It represents the disturbance increment acting on the i-axis during the period from time k to time k+1; Furthermore, the discretized state expansion equation is expressed as: ;in, represents the expanded state vector at time k+1, represents the expanded state vector at time k, represents the input control quantity at time k, represents the external interference vector at time k, represents the state transition matrix, represents the output matrix, represents the interference matrix, and , , , represents the identity matrix, represents the product of the time step and the identity matrix I, represents a matrix; S623, constructing a Lumberg observer based on the discretized state expansion equation and estimating the disturbance; the constructed Lumberg observer is expressed as: ;in, represents the estimated expanded state vector at time k+1, represents the estimated expanded state vector at time k, represents the observer gain matrix, represents the measurement output matrix at time k, represents the output matrix, and ; S624, the disturbance of ESO estimation Feedforward compensation to the input of model predictive control MPC is: ;in, Indicates the control instruction after compensation, Represents the original control instructions for MPC calculation, represents the feedforward compensation gain matrix, represents the disturbance vector of ESO estimation; S625, will Output as the UAV speed control command.

[0038] Secondly, this embodiment provides a UAV trajectory tracking system based on MPC-ESO and path optimization, including: A grid map building module is used to obtain environmental information using a drone and build a grid map based on the environmental information; Planning path module, used to improve the cost function by designing algorithm, and uses the grid map as input data for path planning, and improves the The algorithm plans the path in real time; UAV control module, used to control the attitude and altitude of the UAV; The trajectory tracking module is used to control the UAV trajectory according to the planned path using the model prediction and to perform feedforward compensation on the UAV control variable through the extended state observer to achieve UAV trajectory tracking.

[0039] In the description of the above embodiments, specific features, structures, materials or characteristics may be combined in an appropriate manner in any one or more embodiments or examples.

[0040] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any modifications or substitutions that can be easily conceived by a person skilled in the art within the technical scope disclosed in the present invention should be included in the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be based on the scope of protection of the claims.

Claims

1. A UAV trajectory tracking method based on MPC-ESO and path optimization, characterized in that: include: Using drones to acquire environmental information and creating a grid map based on the environmental information; Improvement through design of cost function algorithm, and uses the grid map as input data for path planning, and improves the The algorithm plans the path in real time; Perform attitude control and altitude position control on the UAV; According to the planned path, the UAV trajectory is controlled by using the model prediction, and the UAV control variable is feedforward compensated through the extended state observer to achieve UAV trajectory tracking.

2. The UAV trajectory tracking method based on MPC-ESO and path optimization according to claim 1 is characterized in that: The improved The algorithm plans the path in real time, including: Design path curvature penalty term; Design the cost function using the path curvature penalty term; Create an open list and a closed list, and add the starting point to the open list; Take the node with the smallest cost function value from the open list. If the node is the end point, start from the end point and trace back to the starting point through the parent node pointer to generate a path point sequence.

3. The UAV trajectory tracking method based on MPC-ESO and path optimization according to claim 2 is characterized in that: After taking out the node with the smallest cost function value from the open list, if the open list is empty, return no solution and traverse the adjacent nodes of the node; Get the cost function value of the adjacent node. If the adjacent node is not in the open list, or the actual cost of the adjacent node is smaller than that of the original node, then update the adjacent node to the parent node, calculate the heuristic function, and insert it into the open list. Repeat until the end is found or the open list is empty and has no solution; If there is a solution, start from the end point and trace back to the starting point through the parent node pointer to generate a sequence of path points.

4. The UAV trajectory tracking method based on MPC-ESO and path optimization according to claim 3 is characterized in that: In the process of designing the cost function using the path curvature penalty term, the cost function is expressed as: ;in, represents the cost function, Indicates the starting point to the current node The actual cost, represents the heuristic function, represents the smoothing weight coefficient, represents the path curvature penalty term.

5. The UAV trajectory tracking method based on MPC-ESO and path optimization according to claim 4 is characterized in that: The design path curvature penalty term includes: Set the current node to n, the predecessor node to n-1, and the successor node to n+1; the coordinates of the current node, predecessor node, and successor node are 、 and ; Defining a vector , ,in, Indicates coordinate points Point to coordinate point The direction vector, Indicates coordinate points Point to coordinate point direction vector; Get the angle between two vectors , according to the angle Get the penalty at the current node n; The penalty at the current node n is expressed as: , represents the smoothing weight coefficient.

6. The UAV trajectory tracking method based on MPC-ESO and path optimization according to claim 5 is characterized in that: The method of using a model to predict and control the trajectory of a UAV includes: Define state quantities and input quantities, and establish a kinematic model; Expand the prediction model and construct the state and input sequence within the prediction time domain N; Formulate an optimization problem and establish an objective function to minimize tracking error and control input penalty; The objective function is converted into a quadratic programming form, and the quadratic programming form solver is called to solve the optimal input sequence online. The control variable is applied as the UAV speed control instruction and then solved again in the next step.

7. The UAV trajectory tracking method based on MPC-ESO and path optimization according to claim 6 is characterized in that: The feedforward compensation of the UAV control variable by the extended state observer includes: The definition of state vector includes the original state vector and disturbance; The disturbance dynamics is modeled as a first-order inertial process, and the discretized state expansion equation is established; Construct a Lumberg observer based on the discretized state expansion equation and estimate the disturbance; The disturbance estimated by the observer is feedforward compensated to the input of the model predictive control and output as the UAV speed control command.

8. The UAV trajectory tracking method based on MPC-ESO and path optimization according to claim 7 is characterized in that: The defined state vector includes the original state vector and the disturbance, which is expressed as: ;in, represents the expanded state matrix, represents the coordinate position of the drone on the x-axis at time k, represents the coordinate position of the drone on the y-axis at time k, represents the coordinate position of the drone on the z-axis at time k, represents the external disturbance to the x-axis position of the drone at time k, represents the external disturbance to the y-axis position of the UAV at time k, represents the external disturbance to the UAV’s z-axis position at time k, Represents the transpose of a matrix; And / or, model the disturbance dynamics as a first-order inertial process expressed as: ;in, , represents the external disturbance of the UAV on the three spatial coordinate axes x, y, and z, represents the estimated value of the total external disturbance acting on the i-axis at time k+1, represents the estimated value of the total external disturbance acting on axis i at time k, It represents the disturbance increment acting on the i-axis during the period from time k to time k+1.

9. The UAV trajectory tracking method based on MPC-ESO and path optimization according to claim 8, characterized in that: The discretized state expansion equation is expressed as: ;in, represents the expanded state vector at time k+1, represents the expanded state vector at time k, represents the input control quantity at time k, represents the external interference vector at time k, represents the state transition matrix, represents the output matrix, represents the interference matrix, and , , , represents the identity matrix, represents the product of the time step and the identity matrix I, represents a matrix; And / or, the constructed Lumberg observer is expressed as: ;in, represents the estimated expanded state vector at time k+1, represents the estimated expanded state vector at time k, represents the observer gain matrix, represents the measurement output matrix at time k, represents the output matrix, and .

10. A UAV trajectory tracking system based on MPC-ESO and path optimization, characterized in that: include: A grid map building module is used to obtain environmental information using a drone and build a grid map based on the environmental information; Planning path module, used to improve the cost function by designing algorithm, and uses the grid map as input data for path planning, and improves the The algorithm plans the path in real time; UAV control module, used to control the attitude and altitude of the UAV; The trajectory tracking module is used to control the UAV trajectory according to the planned path using the model prediction and to perform feedforward compensation on the UAV control variable through the extended state observer to achieve UAV trajectory tracking.

Images