Unmanned aerial vehicle trajectory tracking control method based on inverse dynamics

Abstract

The application discloses a kind of based on inverse dynamics unmanned vehicle trajectory tracking control method, belong to the technical field of unmanned vehicle trajectory control, it includes: obtaining the measured trajectory of aircraft in target area;The measured trajectory is gridded and smoothed;Determine the input and output of unmanned vehicle inverse dynamics based on the measured trajectory after processing;Based on the input and output of unmanned vehicle inverse dynamics, construct unmanned vehicle inverse dynamics model;Based on unmanned vehicle inverse dynamics model, calculate unmanned vehicle control variable;According to unmanned vehicle control variable, verify unmanned vehicle trajectory simulation accuracy.The application constructs the grid function of measured state variable, integrates unmanned vehicle dynamics equation, unmanned vehicle control constraint, realizes efficient target trajectory tracking control variable calculation, ensures that unmanned vehicle is controlled to known trajectory in shortest time Rate construction is completed, while reducing the amount of calculation, optimizing equipment demand.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of unmanned aerial vehicle trajectory control, and particularly relates to an unmanned aerial vehicle trajectory tracking control method based on inverse dynamics. BACKGROUND

[0002] In unmanned aerial vehicle trajectory tracking, the target maneuver involves complex dynamics and control law, and even completely different carriers (vehicles, ships, etc.), which have significant differences in technical characteristics and physical characteristics from unmanned aerial vehicles. For example, in the case of earthquake relief, due to road damage and poor weather environment, visual images cannot be directly used for path planning of unmanned aerial vehicles, and only manned aircraft can be used for one flight, and the trajectory measured by the aircraft is used for trajectory control of the unmanned aerial vehicle to achieve the transportation of a large number of materials into the disaster area. In order to realize high-precision simulation of the known target trajectory of the unmanned aerial vehicle, the dynamic calculation of the control parameters needs to be solved. The traditional control method is difficult to cope with the complex input constituted by the state variable measurement data of different carriers, especially in the case of measurement error, how to ensure the robustness and precision of the control law becomes a technical problem to be solved. SUMMARY

[0003] The application aims at the above-mentioned deficiencies in the prior art, and provides an unmanned aerial vehicle trajectory tracking control method based on inverse dynamics, so as to solve the problem that the traditional control method is difficult to cope with the complex input constituted by the state variable measurement data of different carriers.

[0004] In order to achieve the above-mentioned purpose, the technical scheme adopted by the application is as follows:

[0005] An unmanned aerial vehicle trajectory tracking control method based on inverse dynamics comprises the following steps:

[0006] S1, obtaining a measurement trajectory of an aircraft in a target area;

[0007] S2, performing gridization and smoothing processing on the measurement trajectory;

[0008] S3, determining the input and output of the inverse dynamics of the unmanned aerial vehicle based on the processed measurement trajectory;

[0009] S4, constructing an inverse dynamics model of the unmanned aerial vehicle based on the input and output of the inverse dynamics of the unmanned aerial vehicle;

[0010] S5, calculating the control variable of the unmanned aerial vehicle based on the inverse dynamics model of the unmanned aerial vehicle;

[0011] S6, verifying the trajectory simulation precision of the unmanned aerial vehicle according to the control variable of the unmanned aerial vehicle.

[0012] Further, the S2 comprises the following sub-steps:

[0013] S201. Construct the motion equations of the measurement trajectory;

[0014] S202. Based on the motion equation, the measurement trajectory is converted into a grid function;

[0015] S203. Smooth the measurement trajectory that forms the grid function.

[0016] Furthermore, in S201, the motion equation of the measurement trajectory is expressed as:

[0017]

[0018] In the formula, This represents the rate of change of the state variable of the measured trajectory; Indicates the current state x and control input u The function; Indicates the start time; Indicates system uptime; Indicates the cutoff time;

[0019] In step S202, the measured trajectory is transformed into a grid function based on the equation of motion, thereby converting continuous flight parameters into discrete data points, which is expressed as follows:

[0020]

[0021] In the formula, Represents the discretized state variable values; Indicates in The first time measurement data i One state variable; This represents the r-th time node. τ represents the time step. Indicates the total number of grid points; This represents the total number of state variables. This represents the (r-1)th time node.

[0022] Furthermore, in S203, the measurement trajectory forming the grid function is smoothed, including: using a cubic spline function as a smoothing tool, and determining the cubic spline coefficients by establishing an optimization problem through the least squares method, and finally generating a smooth continuous measurement trajectory.

[0023] The optimization problem is established using the least squares method, and the objective function of this problem is:

[0024]

[0025] In the formula, To optimize the objective function corresponding to the problem; is a cubic spline function.

[0026] Further, the S3 comprises the following steps:

[0027] S301, determine the dynamic model of the unmanned aerial vehicle, and obtain the kinematic parameters of the unmanned aerial vehicle;

[0028] S302, take the state variable of the measured trajectory as the input variable of the inverse dynamics, that is, as the state of the unmanned aerial vehicle, and then inversely solve the control variable of the unmanned aerial vehicle, so that the unmanned aerial vehicle performs the same motion trajectory tracking on the measured trajectory, which is expressed as:

[0029]

[0030] Solve:

[0031] In the formula, is the state of the unmanned aerial vehicle; is the control variable of the unmanned aerial vehicle.

[0032] Further, the S4 comprises the following steps:

[0033] S401, perform the same grid discrete processing on the control variable of the unmanned aerial vehicle as the measured trajectory, and determine the inverse dynamics equation of the unmanned aerial vehicle:

[0034]

[0035] S402, differentiate the inverse dynamics equation, and separately express the part that cannot be differentiated in the remaining equation, and then obtain the complete algebraic equation of the relationship between the control variable and the state vector, which is expressed as:

[0036]

[0037] Among them:

[0038]

[0039] In the formula, represents the second derivative of the state variable; represents the jth component of the state variable; represents the first derivative of the jth component of the state variable; represents the qth component of the state variable, which belongs to the state component that cannot be directly differentiated; is the qth dynamic function, representing the functional relationship between the qth state variable and the current state x and the control input u; represents the s th component of the artificially adjusted control variable; ​​This represents the rate of change of the s-th control variable; Indicates the number of variables;

[0040] S403. The complete algebraic equation in S402 is simplified and discretized using auxiliary functions to construct the UAV inverse dynamics model, which is expressed as follows:

[0041]

[0042] In the formula, Indicates at time The partial derivative coefficients of the control variables, i.e. For the s-th control variable The partial derivatives in The value at that location is represented as ; Represents state variables exist The value at time, Represents control variables exist The value at time;

[0043] This indicates that the s-th control variable at time t r The rate of change;

[0044] This indicates that the i-th state variable at time t r The second derivative;

[0045] s represents the index of the control variable, indicating the s-th control variable;

[0046] Indicates at time The partial derivative coefficients of the state variables, i.e., the function For the j-th state variable The partial derivatives in The value at that location is represented as ;

[0047] Indicates at time The coefficients of the composite function, i.e. exist The value at that point is used to handle state components that cannot be directly differentiated.

[0048] Furthermore, the complete algebraic equation in S402 is simplified using auxiliary functions, including:

[0049] right State vector polynomial in Construct auxiliary functions:

[0050]

[0051] For Supplementary state vector polynomials are used in Auxiliary function construction is performed for:

[0052]

[0053] For Control variable polynomials are used in Auxiliary function construction is performed for:

[0054]

[0055] Wherein:

[0056] In the formula, is the value of the state variable at time , indicating the state quantity of the rth time node of the UAV;

[0057] is the value of the control variable at time , indicating the control input quantity of the rth time node of the UAV;

[0058] is the partial derivative auxiliary function of the state variable, wherein, , indicating the partial derivative of the function with respect to the jth component of the state variable;

[0059] is the supplementary state vector auxiliary function, wherein, , used to handle state components that cannot be directly differentiated;

[0060] is the partial derivative auxiliary function of the control variable, wherein, , indicating the partial derivative of the function with respect to the s th control variable .

[0061] Further, the S5 includes the following sub-steps:

[0062] S501, convert the discrete state variable into first and second derivatives as inputs for inverse dynamics solving, while calculating the UAV control rate;

[0063] The UAV control rate is expressed as:

[0064]

[0065] In the formula, denotes the value of the s-th control variable at time , i.e. the control input at the next time node; denotes the value of the s-th control variable at time , i.e. the control input at the current time node; denotes the interval between adjacent measurement data storages;

[0066] S502, the unmanned aerial vehicle control rate is brought into the unmanned aerial vehicle inverse dynamics model to obtain a simplified unmanned aerial vehicle inverse dynamics model, and then an algebraic equation group of unmanned aerial vehicle control variables is obtained;

[0067] S503, the initial condition control vector is given, and the value of each control variable in the algebraic equation group is sequentially solved, and then the complete control variable at the time is obtained.

[0068] Further, in the S502, the unmanned aerial vehicle control rate is brought into the unmanned aerial vehicle inverse dynamics model to obtain a simplified unmanned aerial vehicle inverse dynamics model, and then an algebraic equation group of unmanned aerial vehicle control variables is obtained;

[0069] The simplified unmanned aerial vehicle inverse dynamics model is represented as:

[0070]

[0071] The algebraic equation group of unmanned aerial vehicle control variables is represented as:

[0072]

[0073] Wherein:

[0074] In the formula, denotes the modified control variable partial derivative coefficient, i.e. the modified value of the auxiliary function obtained in the discretization and simplification process;

[0075] denotes the coefficient of the right end item after simplification, defined as , which contains the combination of second-order derivatives of state variables and other known items;

[0076] denotes the control variable vector at time , i.e. the complete control variable combination at the next time step;

[0077] denotes the value of the first control variable at time ;

[0078] denotes the value of the second control variable at time ;

[0079] denotes the value of the mth control variable at time

[0080] is a coefficient matrix function, which represents the sum of control variable coefficients.

[0081] Further, the S6 comprises the following steps:

[0082] S601, verifying whether the calculated control variable satisfies the actual physical constraints of the UAV, which is represented as:

[0083]

[0084] wherein, denotes the minimum allowable value of the control variable, corresponding to the minimum control input limit of the UAV design; denotes the maximum allowable value of the control variable, corresponding to the maximum control input limit of the UAV design control variable; is a control variable, which represents the control input within the interval

[0085] S602, applying the control variable meeting the actual physical constraints of the UAV to the UAV motion equation, and performing UAV trajectory simulation;

[0086] S603, calculating the absolute error and relative error between the UAV trajectory and the measured trajectory, the smooth trajectory, and further verifying the simulation accuracy of the UAV trajectory.

[0087] The UAV trajectory tracking control method based on inverse dynamics provided by the present application has the following beneficial effects:

[0088] The present application constructs a grid function of the measured state variable, integrates the UAV dynamics equation and the UAV control constraints, realizes efficient target trajectory tracking control variable calculation, ensures the completion of the control rate construction of the UAV on the known trajectory in the shortest time, reduces the calculation amount, and optimizes the equipment demand.

[0089] ​​The present application significantly improves the accuracy and efficiency of unmanned aerial vehicle simulation of known measurement trajectory through a two-stage method. The first stage of smoothing optimization effectively eliminates noise in the measurement data, reduces numerical differentiation error, and ensures the realizability of the input data. The second stage significantly reduces the computational complexity by converting the motion equation into a recursive formula of inverse dynamics control problem, which is suitable for on-board real-time control system and significantly improves the accuracy compared with traditional methods. The control variable generation process is fast and meets the real-time demand, and is fully compatible with the design constraints of unmanned aerial vehicles (such as maximum pitch angle). This method is suitable for mixed formation, camouflage and reconnaissance scenes, supports helicopter simulation with different technical characteristics, and has wide military and civilian application prospects. In addition, this method is robust to data noise and can be extended to complex maneuver simulation, providing an efficient solution for unmanned aerial vehicle autonomous control. BRIEF DESCRIPTION OF DRAWINGS

[0090] Figure 1 The flowchart of the unmanned aerial vehicle trajectory tracking control method based on inverse dynamics of embodiment 1 of the present application.

[0091] Figure 2 The image of height measurement data of manned helicopter of embodiment 2, wherein the grid step is 0.2s.

[0092] Figure 3 The smoothing result of the three times spline approximation of the height measurement data of embodiment 2.

[0093] Figure 4 The image of the change of the control rate of the main rotor pitch angle of the unmanned aerial vehicle of embodiment 2.

[0094] Figure 5 The comparison of the trajectory of the unmanned aerial vehicle and the measurement trajectory of embodiment 2.

[0095] Figure 6 The comparison of the trajectory of the unmanned aerial vehicle and the smoothed trajectory of embodiment 2.

[0096] Figure 7 The relative error percentage of the trajectory simulation of embodiment 2. DETAILED DESCRIPTION

[0097] The specific embodiments of the present application are described below to facilitate the understanding of the present application by those skilled in the art, but it should be clear that the present application is not limited to the scope of the specific embodiments, and for those skilled in the art, it is obvious that various changes are within the spirit and scope of the present application defined and determined by the appended claims, and all inventions utilizing the concept of the present application are within the scope of protection.

[0098] Embodiment 1

[0099] The UAV trajectory tracking control method based on inverse dynamics in this embodiment is robust to data noise and can be extended to the simulation of complex maneuvers, providing an efficient solution for UAV autonomous control. (Reference) Figure 1 Specifically, it includes the following:

[0100] S1. Obtain the measurement trajectory of the aircraft in the target area;

[0101] In the target area, a manned aircraft will conduct a flight and use the aircraft to obtain a measurement trajectory.

[0102] S2. The measurement trajectory is gridded and smoothed, which specifically includes the following:

[0103] S201. Construct the motion equations of the measurement trajectory, which specifically include:

[0104] Measurement parameter settings:

[0105] Time step: τ (sampling interval);

[0106] Measurement duration: t (total sampling time);

[0107] Total number of measurement points: k (number of sample points);

[0108] State variable recording: Obtain key flight parameters of the target aircraft;

[0109] Using onboard GPS, pilot control stick output information, or other positioning devices, the rate of change of the target aircraft's state variables can be obtained. The measurement trajectory defines a vector of state variables. Used to represent phase coordinates (such as position, velocity, angular velocity); defines a vector of control variables. Used to represent control inputs (such as rotor angle and thrust), and the rate of change of state variables. Equal to function State and control output The function value is used to construct the motion equation of the measured trajectory, which is expressed as:

[0110]

[0111] In the formula, This represents the rate of change of the state variable of the measured trajectory; Indicates the current state x and control input u The function; Indicates the start time; Indicates system uptime; Indicates the cutoff time.

[0112] The initial conditions for the equation of motion are: The termination condition is Where U is the allowed set of control variables.

[0113] S202. Based on the equation of motion, the measured trajectory is transformed into a grid function. The grid function converts continuous flight parameters into discrete data points, providing a standardized data format for subsequent numerical calculations.

[0114] Specifically, trajectory data is represented using a constant time step τ (e.g., 0.2 s). A constant step size facilitates numerical differencing and interpolation, reducing computational complexity. The step size is chosen based on the Nyquist sampling theorem to ensure the capture of the trajectory's main dynamic frequencies. The raw data is structured into a grid function suitable for numerical processing, unifying the time resolution and laying the foundation for smoothing and derivative calculations.

[0115] The process of converting the measurement trajectory into a grid function, and then converting the continuous measurement state trajectory into a series of discrete data points, is represented as follows:

[0116]

[0117] In the formula, Represents the discretized state variable values; Indicates in The first time measurement data i One state variable; This represents the r-th time node. ; Indicates the total number of grid points; Indicates the total number of state variables; This represents the (r-1)th time node.

[0118] For a specific moment and step size Define:

[0119]

[0120]

[0121] Accuracy requirements: ,in, The highest frequency component of the target trajectory;

[0122] Computational efficiency: Limit the total number of computational grid points, where, This represents the maximum number of grid points. This represents the actual number of grid points.

[0123] Storage Limit: Data volume ≤ Onboard storage capacity limit;

[0124] in the grid function is a physical quantity describing the current state of the system, u is a control quantity (control input) that can be artificially adjusted, which is expressed as:

[0125]

[0126]

[0127] S203, the measurement trajectory forming the grid function is smoothed, the measurement noise and measurement error in the trajectory are processed, so that the trajectory is as smooth as possible, and the complexity of the subsequent calculation steps is reduced;

[0128] Specifically, a cubic spline function is used as a smoothing tool, and an optimization problem is established by using the least square method to determine the spline function coefficients, and finally a smooth continuous measurement trajectory is generated.

[0129] Smooth method selection:

[0130] A cubic spline function is used as a smoothing tool;

[0131] The objective function corresponding to the optimization problem is :

[0132]

[0133] In the formula, Indicates a cubic spline function.

[0134] Spline coefficient solving:

[0135] The polynomial coefficients of the cubic spline function are determined by solving the linear equation set by the least square method, and the complete spline function is constructed.

[0136] Smooth trajectory generation:

[0137] A smooth continuous measurement trajectory is generated on the time interval , and a small enough value is selected to meet the accuracy requirements of subsequent numerical differentiation.

[0138] S3, based on the processed measurement trajectory, the input and output of the unmanned aerial vehicle inverse dynamics are determined, which specifically includes the following contents:

[0139] In S2, the measurement trajectory has been optimized to conform to the grid function of the unmanned aerial vehicle dynamics, and then the state variable is taken as the input of the unmanned aerial vehicle inverse dynamics, and the unmanned aerial vehicle parameter definition and inverse dynamics problem modeling preparation are carried out;

[0140] S301, determine the dynamics model of the unmanned aerial vehicle, and obtain the kinematic parameters of the unmanned aerial vehicle (number of rotors, rotor area, rotor speed, body area, weight, etc.); the kinematics of the unmanned aerial vehicle is modeled by using the parameters of the kinematics of the unmanned aerial vehicle and its mathematical formula (rotor thrust coefficient, air induction speed, resistance of the unmanned aerial vehicle in motion, lift coefficient, etc.);

[0141] S302, the state variables of the measured trajectory As the input variable of inverse dynamics, that is, as the state of the unmanned aerial vehicle, the control variable of the unmanned aerial vehicle is solved in reverse, so that the unmanned aerial vehicle can track the same motion trajectory of the measured trajectory, which is expressed as:

[0142]

[0143] , Solve: ,

[0144] In the formula, is the state of the unmanned aerial vehicle; is the control variable of the unmanned aerial vehicle.

[0145] S4, based on the input and output of the inverse dynamics of the unmanned aerial vehicle, the inverse dynamics model of the unmanned aerial vehicle is constructed, which specifically includes the following contents:

[0146] S401, first, unify the time grid construction, define the control action required by the twin plan of the unmanned aerial vehicle in the form of the same function as the state variable of the measured trajectory: perform the same discrete operation on the inverse dynamics model of the unmanned aerial vehicle as the input measured trajectory, ensure that the measured trajectory data and the control variable of the unmanned aerial vehicle adopt the same time discretization (position variable and control variable), and can directly use the grid function of the measured trajectory as the system input, and the related parameters of the unmanned aerial vehicle are expressed as follows:

[0147] Unmanned aerial vehicle state variable (consistent with the grid function in S202):

[0148]

[0149] Unmanned aerial vehicle control input:

[0150]

[0151] Similarly, determine the motion equation of the unmanned aerial vehicle (the same as the motion equation of the measured trajectory in S201):

[0152]

[0153] S402, partial derivative operation is performed on the above motion equation in inverse dynamics, and the n first-order inverse dynamics equation groups are converted to m second-order equation groups, and the first m equations in the inverse dynamics equation are derived with respect to time:

[0154]

[0155] And using the chain rule to expand:

[0156]

[0157] Thus, the relationship between the control quantity and the state vector is changed into an algebraic equation.

[0158] Supplement the part that cannot be derived in the above equation, and further perfect the whole equation; the explicit description of the state components by derivation is only mathematically completely true, but some state components may only affect the rest of the equation form of the dynamic behavior through the current value of the state and the control (not the derivative, so the expression is directly represented by the measured quantity, without derivation with respect to time), so the whole component is divided into two parts, the first m components can be represented by the derivative, and the rest of the components are represented separately:

[0159]

[0160] The expression of the above component is brought into the algebraic equation, and the complete algebraic equation is obtained, which is represented as:

[0161]

[0162] Among them:

[0163] In the formula, represents the second derivative (acceleration) of the state variable; represents the jth component of the state variable, which describes the physical quantity of the current state of the system; represents the first derivative (velocity) of the jth component of the state variable; represents the qth component of the state variable, which belongs to the state component that cannot be directly represented by derivation; represents the qth dynamic function, which describes the function relationship between the qth state variable and the current state x and the control input u; represents the s th component of the artificially adjusted control quantity; represents the change rate of the s th control variable, also known as "control rate"; represents the total number of state variables;

[0164] S403, auxiliary function simplification and discretization are performed on the complete algebraic equation in S402;

[0165] At any time: ; is the state variable at time , is the control variable at time ;

[0166] The auxiliary function simplification of the complete algebraic equation of the relationship between the control variable and the state vector in S402 includes:

[0167] The auxiliary function construction of the state vector polynomial at :

[0168]

[0169] The auxiliary function construction of the supplementary state vector polynomial at :

[0170]

[0171] The auxiliary function construction of the control variable polynomial at :

[0172]

[0173] Wherein:

[0174] In the formula, represents the value of the state variable at time , and at this time is , which represents the state variable of the unmanned aerial vehicle at the rth time node;

[0175] represents the value of the control variable at time , and at this time is , which represents the control input variable of the unmanned aerial vehicle at the rth time node;

[0176] represents the state variable partial derivative auxiliary function, that is , which represents the partial derivative of the function with respect to the jth state variable ;

[0177] represents the supplementary state vector auxiliary function, that is , which is used to process the state components that cannot be directly derived;

[0178] ​​​ denotes the partial derivative of the control variable, i.e. denotes the partial derivative of the control variable, i.e.

[0179] Substitute the auxiliary function into the complete algebraic equation, we get:

[0180]

[0181] and move the control variable polynomial in it to the left side of the equation, finally get the expression of the UAV inverse dynamics model:

[0182]

[0183] where, denotes the partial derivative of the control variable at time , i.e. the dynamics function denotes the value of the partial derivative of the dynamics function with respect to the s-th control variable at ;

[0184] s denotes the index of the control variable, which denotes the s-th control variable;

[0185] denotes the partial derivative of the state variable at time , i.e. the dynamics function denotes the value of the partial derivative of the dynamics function with respect to the j-th state variable at ;

[0186] denotes the composite function coefficient at time , i.e. the value of at, which is used to handle state components that cannot be directly differentiated.

[0187] S5, based on the inverse dynamics model of the UAV, the control variables of the UAV are calculated, which includes the following contents:

[0188] S501, in S2, the is given in the form of a discrete grid function, is the interval between adjacent measurement data storage, since only discrete data points in the grid function , there is no continuous state function , the derivative velocity and acceleration ​​; thus, discrete state data is converted to first order derivative (velocity) and second order derivative (acceleration) to provide necessary input for inverse dynamics problem solving:

[0189] velocity at time instant t is calculated as:

[0190]

[0191] where, denotes first order derivative (velocity) of the i-th state variable at time instant t r

[0192] denotes discrete value of the i-th state variable at the r-th time node;

[0193] denotes discrete value of the i-th state variable at the (r+q)-th time node;

[0194] acceleration at time instant t is calculated as:

[0195]

[0196] where, denotes second order derivative (acceleration) of the i-th state variable at time instant t r

[0197] denotes discrete value of the i-th state variable at the (r+2)-th time node;

[0198] denotes discrete value of the i-th state variable at the (r+1)-th time node;

[0199] denotes discrete value of the i-th state variable at the r-th time node;

[0200] derivative of control variable, "control rate", is calculated as:

[0201]

[0202] where, denotes value of the s-th control variable at time instant , i.e. control input at next time node; denotes value of the s-th control variable at time instant , i.e. control input at current time node; denotes interval between adjacent measurement data storage;

[0203] ​​​​Directly using adjacent discrete control variables and to estimate the derivative to form a unified discrete framework for subsequent substitution into the linear equation set.

[0204] S502, the control rate is brought into the UAV inverse dynamics model, and is defined as:

[0205]

[0206] Further, we get:

[0207]

[0208] The simplified UAV inverse dynamics model is obtained:

[0209]

[0210] Thus, a series of recursive linear algebraic equations about unknown control variables are obtained, and an algebraic equation set about solving control variables is obtained:

[0211]

[0212] Where:

[0213] For example, the value of the control variable at time : ;

[0214] In the formula, represents the modified control variable partial derivative coefficient, that is, the modified value of the auxiliary function obtained in the process of discretization and simplification, which is used to establish recursive linear algebraic equations;

[0215] represents the coefficient of the right end item after simplification, and is defined as , which contains the second derivative of the state variable and the combination of other known items;

[0216] represents the control variable vector at time , that is, the complete control variable combination at the next time step;

[0217] represents the value of the first control variable at time ;

[0218] represents the value of the second control variable at time ;

[0219] denotes the value of the mth control variable at time

[0220] denotes the coefficient matrix function defined as , which is used to represent the sum of the control variable coefficients.

[0221] S503, given the initial condition control vector, the value of each control variable in the algebraic equation set is solved in turn, and then the complete control variable at this time is obtained.

[0222] For a given initial condition control vector, the value of each control variable is solved in turn, and they are combined to obtain the complete control variable at this time

[0223]

[0224] At time t, different control variable combinations obtain the complete control variable at time t; then use continue to calculate .

[0225] S6, according to the unmanned aerial vehicle control variable, verify the unmanned aerial vehicle trajectory simulation accuracy, which specifically includes the following contents:

[0226] S601, control variable physical constraint verification: according to the unmanned aerial vehicle design specification and operation safety requirement, verify whether the calculated control variable meets the actual physical constraint condition, which is as follows:

[0227]

[0228] In the formula, denotes the minimum allowable value of the control variable, corresponding to the minimum control input limit of the unmanned aerial vehicle design; denotes the maximum allowable value of the control variable, corresponding to the maximum control input limit of the unmanned aerial vehicle design control variable; is the control variable, which represents the control input in the interval

[0229] S602, apply the control variable meeting the actual physical constraint of the unmanned aerial vehicle to the unmanned aerial vehicle motion equation, and carry out unmanned aerial vehicle trajectory simulation;

[0230] S603, calculate the absolute error and relative error between the unmanned aerial vehicle trajectory and the measured trajectory, smooth trajectory, and then verify the unmanned aerial vehicle trajectory simulation accuracy;

[0231] Also includes verifying whether the control vector meets the unmanned aerial vehicle design constraints (such as the maximum pitch angle).

[0232] Embodiment 2​​

[0233] This embodiment describes the method in embodiment 1 with specific cases;

[0234] This embodiment uses MQ-8C Fire Scout type UAV-twin to simulate the vertical take-off and hovering maneuver feasibility of Sikorsky S-92 type manned helicopter; through the inverse dynamics method proposed in the application, high-precision simulation of light unmanned aerial vehicle motion to heavy manned helicopter is realized, reference Figures 2-7 , which specifically includes the following contents:

[0235] S1, griding and smoothing processing of the measured trajectory, which specifically includes the following contents:

[0236] S101, data acquisition;

[0237] Data acquisition is performed on the state variable measurement of the Sikorsky S-92 type manned helicopter, and height-time original trajectory data is obtained;

[0238] Reference Figure 2 , the grid step τ = 0.2 s is adopted, and the manned helicopter height measurement data is obtained:

[0239]

[0240] Among them: is the value of the measurement data at time

[0241]

[0242] k = 60 (total number of measurement points, corresponding to 12 seconds of flight time);

[0243] τ = 0.2 s (sampling interval) that is, the step.

[0244] S102, data smoothing processing;

[0245] Due to the existence of significant errors in the original measurement data, reference Figure 3 , a cubic spline function is used for smoothing processing:

[0246] Smoothing method: select a cubic function as a smoothing spline, use the least square method to determine the polynomial coefficient, eliminate measurement noise and improve data quality.

[0247] Smoothing effect: the original data has a measurement error of ±2m; the smoothed data has an error of less than ±0.5m.

[0248] ​​S2, determine the parameters related to the UAV, determine the inverse dynamics system and input and output, which specifically includes the following:

[0249] S201, determine the parameters related to the UAV, see Table 1 and Table 2;

[0250] Table 1 - manned helicopter parameters (Sikorsky S-92)

[0251]

[0252] Table 2 - UAV parameters (MQ-8C Fire Scout)

[0253]

[0254] S202, given the measurement state trajectory As the input variable of inverse dynamics, that is, as the state of the UAV , inverse solution of the UAV control variable , so that the UAV tracks the same motion trajectory; it is expressed as:

[0255] Given: , → solution: , ;

[0256] S3, mathematical modeling of inverse dynamics and derivation of UAV control variables, which specifically includes the following:

[0257] S301, inverse dynamics equation is established;

[0258] For the mathematical model of the center of mass motion of the UAV twin, define the state variable:

[0259]

[0260] Where: X1= V: represents the rate of change of height (vertical velocity); X2= H: represents the barometric height; u = φ: represents the collective pitch angle of the main rotor (control variable);

[0261] S302, mathematical formula modeling;

[0262] Lift formula:

[0263] .

[0264] Resistance formula:

[0265]

[0266] In the formula, represents the thrust coefficient;

[0267] Parameter values: (Air density), (Resistance coefficient and area product);

[0268] Inverse dynamics equations:

[0269]

[0270]

[0271] where, is the main rotor lift; X(H, V) is the air resistance along the OY axis moving upward; F is the resultant force of the movement system in the vertical direction; G is the gravity; M is the mass of the UAV; denotes the first derivative of the first state variable, i.e. the vertical acceleration; denotes the derivative of the vertical velocity with respect to time, i.e. the vertical acceleration; denotes the first derivative of the second state variable, i.e. the rate of change of height; denotes the derivative of the height with respect to time, i.e. the rate of change of height (vertical velocity).

[0272] Thrust coefficient calculation:

[0273]

[0274] where, is the slope of the lift curve; denotes the rotor speed; is the dimensionless of the UAV ascending speed; is the vertical component of the main rotor induced velocity.

[0275] S303, equation partial derivative operation;

[0276] The state variables are reordered to obtain:

[0277] X = (X1, X2) = (V, H)

[0278] Inverse dynamics equations contain control variables , whose derivatives with respect to time are obtained as control rates :

[0279]

[0280] S304, supplementary equations;

[0281] Directly give the state variables that cannot be directly represented by derivatives:

[0282]

[0283] Bringing it into the S301 inverse dynamics equation gives the complete expression:

[0284]

[0285] S305, auxiliary function simplification

[0286] Define auxiliary functions:

[0287]

[0288]

[0289]

[0290] In the formula, denotes the state variable partial derivative auxiliary function, which is used to describe the influence of thrust and drag on speed; denotes the composite state auxiliary function, which is used to process the coupling effect of height and speed; denotes the control variable partial derivative auxiliary function, which describes the influence of the pitch angle on the thrust;

[0291] The value of each auxiliary function at the moment of is:

[0292]

[0293]

[0294] ;

[0295] In the formula, denotes the state variable partial derivative coefficient at tr;

[0296] denotes the composite function coefficient at tr;

[0297] denotes the control variable partial derivative coefficient at tr;

[0298] Bring all auxiliary functions into the S303 inverse dynamics equation expression to get:

[0299]

[0300] In the formula, denotes the first derivative of acceleration at tr, vertical acceleration; denotes the acceleration at tr, i.e. vertical speed denotes the rate of change of the control variable (main rotor collective pitch angle) at tr, i.e. the pitch angle change rate;

[0301] The control variable polynomial is expressed separately:

[0302]

[0303] .

[0304] Expand Get:

[0305] .

[0306] From this, the complete inverse dynamics model is obtained, and the control variable expression is obtained.

[0307] S4, solving the inverse dynamics problem, specifically including the following contents:

[0308] S401, define the grid function of the unmanned aerial vehicle control variable:

[0309]

[0310] Ensure the same length of discretization as the grid function usage.

[0311] S402, numerical differentiation calculation:

[0312] State variable derivative:

[0313]

[0314]

[0315]

[0316] In the formula, The speed at time t; Indicates the measured height value at the (r+1)th time node; Indicates the measured height value at the rth time node; Indicates the measured height value at the (r+2)th time node; Indicates the measured height value at the (r+3)th time node;

[0317] Control variable derivative:

[0318]

[0319] In the formula, Indicates the main rotor pitch angle control value at the (r+1)th time node; Indicates the main rotor pitch angle control value at the rth time node. ​

[0320] S403, Recurrence equation establishment:

[0321] Substitute the numerical differentiation formula into the control equation S304 to obtain the recurrence relation:

[0322]

[0323]

[0324] where, denotes the modified control variable partial derivative coefficient, i.e., the partial derivative value of the thrust function with respect to the pitch angle at the rth time node, which is used to establish the recurrence relation between the control variable and the state variable;

[0325] denotes the modified state variable partial derivative coefficient, i.e., the partial derivative value of the thrust and drag functions with respect to the velocity at the rth time node, which describes the influence of velocity on system dynamics;

[0326] denotes the modified composite function coefficient, i.e., the product of the partial derivative of the thrust and drag functions with respect to the altitude and the velocity at the rth time node, which is used to handle the altitude-velocity coupling effect;

[0327] Perform bracket expansion and move the control variable to the left side of the equation to obtain:

[0328]

[0329] Since the measurement is cut off at , and the needed to calculate is unknown, in order to correctly perform the numerical differentiation operation, it is assumed that:

[0330]

[0331]

[0332] In this way, the complete control variable expression is obtained.

[0333] S404, Parameter calculation:

[0334] , , : calculated according to the auxiliary function at given and values, initial conditions: , constraint conditions: .

[0335] S5, Accuracy verification and evaluation, referenceFigure 5 、 Figure 6 and Figure 7 , which specifically includes the following:

[0336] S501, trajectory simulation

[0337] using the calculated control law , by integrating the motion equation group, the unmanned aerial vehicle height function is obtained .

[0338] S502, calculate the absolute error and relative error between the unmanned aerial vehicle trajectory and the measured trajectory, the smoothed trajectory, and the error analysis is shown in Table 3, which is as follows:

[0339] Table 3 - Error Analysis

[0340]

[0341] S503: constraint verification;

[0342] The verification result shows that the pitch angle at all times satisfies 0° ≤ φ(t) ≤ 9°, and the unmanned aerial vehicle is suitable for simulating this maneuvering action, and the control law is within the design constraint range.

[0343] Although the specific embodiments of the application are described in detail with reference to the accompanying drawings, it should not be understood as limiting the scope of protection of the patent. Various modifications and variations made by those skilled in the art within the scope described in the claims are still within the scope of protection of the patent.

Claims

1. A UAV trajectory tracking control method based on inverse dynamics, characterized in that, Includes the following steps: S1. Obtain the measurement trajectory of the aircraft in the target area; S2. Grid and smooth the measurement trajectory; S3. Based on the processed measurement trajectory, determine the input and output of the UAV inverse dynamics; S4. Construct an inverse dynamics model for UAVs based on the inputs and outputs of UAV inverse dynamics; S4 includes the following steps: S401. Perform the same gridded discretization process on the UAV control variables as on the measured trajectory, and determine the UAV's inverse dynamics equations: In the formula, The first part representing the measurement trajectory i Rate of change of each state variable; Indicates the current state x and control input u The function; Indicates the start time; Indicates system uptime; Indicates the cutoff time; S402. Differentiate the inverse dynamics equations and separately represent the non-differentiable parts of the remaining equations to obtain the complete algebraic equations relating the control quantity and the state vector, which are expressed as: in: In the formula, The second derivative of the state variable; This represents the j-th component of the state variable; This represents the first derivative of the j-th component of the state variable; This represents the q-th component of the state variable, which is a state component that cannot be directly differentiated. Let be the q-th dynamic function, representing the functional relationship between the q-th state variable and the current state x and the control input u; This represents the s-th component of the manually adjusted control quantity; This represents the rate of change of the s-th control variable; Indicates the number of variables; Indicates the total number of state variables; S403. The complete algebraic equation in S402 is simplified and discretized using auxiliary functions to construct the UAV inverse dynamics model, which is expressed as follows: In the formula, Indicates at time The partial derivative coefficients of the control variables, i.e. For the s-th control variable The partial derivatives in The value at that location is represented as ; Represents state variables exist The value at time, Represents control variables exist The value at time; This indicates that the s-th control variable at time t The rate of change; This indicates that the i-th state variable at time t The second derivative; s represents the index of the control variable, indicating the s-th control variable; Indicates at time The partial derivative coefficients of the state variables, i.e., the function For the j-th state variable The partial derivatives in The value at that location is represented as ; Indicates at time The coefficients of the composite function, i.e. exist The value at that point is used to handle state components that cannot be directly differentiated; S5. Calculate the control variables of the UAV based on the UAV inverse dynamics model; S6. Verify the accuracy of the UAV trajectory simulation based on the UAV control variables.

2. The UAV trajectory tracking control method based on inverse dynamics according to claim 1, characterized in that, S2 includes the following steps: S201. Construct the motion equations of the measurement trajectory; S202. Based on the motion equation, the measurement trajectory is converted into a grid function; S203. Smooth the measurement trajectory that forms the grid function.

3. The UAV trajectory tracking control method based on inverse dynamics according to claim 2, characterized in that, In S201, the motion equation of the measurement trajectory is expressed as: In step S202, the measured trajectory is transformed into a grid function based on the equation of motion, thereby converting continuous flight parameters into discrete data points, which is expressed as follows: In the formula, Represents the discretized state variable values; Indicates in The first time measurement data i One state variable; This represents the r-th time node. τ represents the time step. Indicates the total number of grid points; This represents the (r-1)th time node.

4. The UAV trajectory tracking control method based on inverse dynamics according to claim 3, characterized in that, In step S203, the measurement trajectory forming the grid function is smoothed, including: using a cubic spline function as a smoothing tool, and establishing an optimization problem through the least squares method to determine the cubic spline coefficients, and finally generating a smooth continuous measurement trajectory. The optimization problem is established using the least squares method, and the objective function of this problem is: In the formula, To optimize the objective function corresponding to the problem; It is a cubic spline function.

5. The UAV trajectory tracking control method based on inverse dynamics according to claim 4, characterized in that, S3 includes the following steps: S301. Determine the dynamic model of the UAV and obtain its kinematic parameters; S302. The state variables of the measured trajectory are used as input variables of inverse dynamics, i.e., as the UAV state, and then the UAV control variables are solved in reverse to enable the UAV to track the same motion trajectory as the measured trajectory. This can be expressed as: , →Solution: , In the formula, The drone is in a certain state. These are the control variables for the drone.

6. The UAV trajectory tracking control method based on inverse dynamics according to claim 5, characterized in that, The complete algebraic equation in S402 is simplified using auxiliary functions, including: right State vector polynomial in Construct auxiliary functions: right Supplementing the state vector polynomial in Construct auxiliary functions: right Control variable polynomial in Construct auxiliary functions: in: In the formula, For state variables at time... The value of represents the state quantity of the UAV at the r-th time node; To control the variable at time 10:00 The value of represents the control input of the UAV at the r-th time node; Let be an auxiliary function for the partial derivatives of the state variables, where , representing a function For the j-th component of the state variable The partial derivatives; To supplement the state vector auxiliary function, where, It is used to handle state components that cannot be directly differentiated; Let be an auxiliary function for controlling the partial derivatives of the variables, where , representing a function For the s-th control variable The partial derivatives of .

7. The UAV trajectory tracking control method based on inverse dynamics according to claim 6, characterized in that, S5 includes the following steps: S501, convert discrete state variables The values ​​are converted to first and second derivatives to serve as inputs for the inverse dynamics solution, while simultaneously calculating the UAV control law. The control rate of a drone is expressed as: In the formula, This indicates that the s-th control variable is at time t. The value of is the control input for the next time node; This indicates that the s-th control variable is at time t. The value of is the control input at the current time point; S502. Substitute the UAV control law into the UAV inverse dynamics model to obtain the simplified UAV inverse dynamics model, and then obtain the algebraic equation system of the UAV control variables. S503. Given the initial condition control vector, solve the value of each control variable in the algebraic equation system in turn to obtain the complete control variables at that moment.

8. The UAV trajectory tracking control method based on inverse dynamics according to claim 7, characterized in that, In step S502, the UAV control law is substituted into the UAV inverse dynamics model to obtain a simplified UAV inverse dynamics model, which in turn yields a set of algebraic equations for the UAV control variables. The simplified UAV inverse dynamics model is expressed as follows: The algebraic equations representing the control variables of the UAV are as follows: in: In the formula, This represents the modified partial derivative coefficients of the control variables, i.e., the auxiliary function obtained during discretization and simplification. Correction value; The coefficient of the simplified right-hand side is defined as follows: It includes the combination of the second derivative of the state variable and other known terms; Indicates time The control variable vector, i.e., the complete combination of control variables for the next time step; This indicates the time of the first control variable. The value; This indicates the second control variable at time [time]. The value; This indicates that the m-th control variable at time t is... The value; This is a coefficient matrix function, representing the sum of the coefficients of the control variables.

9. The UAV trajectory tracking control method based on inverse dynamics according to claim 3, characterized in that, S6 includes the following sub-steps: S601. Verify whether the calculated control variables satisfy the actual physical constraints of the UAV, expressed as follows: In the formula, This represents the minimum allowable value of the control variable, corresponding to the minimum control input limit in UAV design; This represents the maximum permissible value of the control variable, corresponding to the maximum control input limit of the control variable in UAV design; To control variables, representing the interval Internal control input; S602. Apply control variables that conform to the actual physical constraints of the UAV to the UAV's motion equations and perform UAV trajectory simulation. S603. Calculate the absolute and relative errors between the UAV trajectory and the measured trajectory and smooth trajectory, and then verify the accuracy of the UAV trajectory simulation.

Images