Trajectory tracking control method and system for parent-child unmanned aerial vehicle air release

Abstract

The application discloses a trajectory tracking control method and system for parent-child type unmanned aerial vehicle air release, and belongs to the technical field of unmanned aerial vehicle control. The method steps are as follows: based on the release state of the parent-child type unmanned aerial vehicle, time-varying parameters are calculated in real time, and a time-varying dynamics model is constructed; a position three-order linear extended state observer and three attitude second-order linear extended state observers in parallel are constructed respectively; the total thrust size and the expected attitude are obtained; the control moment is calculated; the total cost function is constructed, and the candidate solution vector of the controller parameter constructed based on the particle swarm optimization algorithm is iteratively optimized to obtain real-time target controller parameters; and the trajectory tracking controller is dynamically adjusted based on the real-time target controller parameters to continuously track the trajectory of the parent-child type unmanned aerial vehicle during the dynamic release of the child machine. The system continuously tracks the trajectory based on the above method. The application solves the problems of insufficient accuracy and stability of the parent-child type unmanned aerial vehicle trajectory tracking control.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of unmanned aerial vehicle control, and particularly relates to a trajectory tracking control method and system for aerial release of a mother-daughter unmanned aerial vehicle. BACKGROUND

[0002] With the rapid development of low-altitude economy, emergency rescue, intelligent logistics and other fields, a multi-unmanned aerial vehicle coordination system has shown great application value. As a new type of aerial operation platform, a mother-daughter unmanned aerial vehicle (Mothership-Daughter UAV System) can realize multi-angle, wide-range and rapid task deployment by virtue of the cooperative advantages of "long endurance of the mother vehicle" and "high flexibility of the daughter vehicle". However, the technical bottleneck of the system is concentrated in the flight stability and trajectory tracking accuracy of the mother vehicle during the dynamic release of the daughter vehicle in the air. In order to ensure the safety of the mother vehicle itself and guarantee the accurate deployment of the daughter vehicle at the correct position, the controller must maintain high-precision tracking of the expected trajectory throughout the entire task cycle before, during and after the release of the daughter vehicle.

[0003] When the mother vehicle dynamically releases the daughter vehicle in the air, it will encounter extremely severe transient disturbances and internal system dynamic changes, which constitute a core challenge to the trajectory tracking control. These compound disturbances mainly manifest in three aspects: (1) step mutation of the system physical properties, i.e., the total mass, center of mass position and moment of inertia matrix of the mother vehicle will change instantaneously and stepwise; (2) mechanical recoil and aerodynamic mutation impact, including the instantaneous impact generated by the release mechanism and the complex, non-analytical aerodynamic disturbance force and moment caused by the separation of the daughter vehicle; (3) strong coupling nonlinearity problem, i.e., the changes in the center of mass and inertia significantly enhance the coupling effect between the translation and rotation channels. However, existing trajectory tracking control technologies are difficult to effectively solve the trajectory maintenance problem in this scenario, mainly due to the limitations in the applicability of the model or strategy. For dynamic modeling, existing researches are mostly focused on the suspended load model, variable mass model, or fixed-wing air-drop model. In terms of control strategy, the generalization ability of traditional intelligent control strategies is difficult to cope with the instantaneous large changes in internal parameters, while the classic model-based control strategy is difficult to quickly converge under instantaneous step disturbances, and the active disturbance rejection control has inherent compensation lag in dealing with step changes in known parameters. Therefore, there is an urgent need for a compound control method that can actively handle known parameter mutations and real-time suppress unknown transient impacts. SUMMARY

[0004] In view of the above deficiencies in the prior art, the trajectory tracking control method and system for aerial release of a mother-daughter unmanned aerial vehicle provided by the present application solve the problems of insufficient precision and stability of trajectory tracking control of a mother-daughter unmanned aerial vehicle.

[0005] In order to achieve the above-mentioned application purposes, the technical solution adopted by the present application is as follows:

[0006] In one aspect, the application provides a trajectory tracking control method for the aerial release of a parent-child unmanned aerial vehicle, comprising the following steps:

[0007] S1. Based on the release state of the parent-child unmanned aerial vehicle, real-time calculation of time-varying parameters is performed, and a time-varying dynamics model is constructed;

[0008] S2. A position third-order linear extended state observer and three attitude second-order linear extended state observers in parallel are constructed respectively;

[0009] S3. Based on the time-varying dynamics model and the position third-order linear extended state observer, a compound control law is derived and a thrust is calculated to obtain the total thrust size and the expected attitude;

[0010] S4. The command angular acceleration is calculated according to the expected attitude, and the error signal is defined, and based on the three attitude second-order linear extended state observers in parallel, the nonlinear dynamic inverse and the time-varying dynamics model, the control moment is calculated;

[0011] S5. The total cost function is constructed according to the total thrust size and the control moment, and the candidate solution vector constructed by the controller parameters of the trajectory tracking controller is iteratively optimized based on the particle swarm optimization algorithm to obtain the real-time target controller parameters;

[0012] S6. Based on the real-time target controller parameters, the trajectory tracking controller is dynamically adjusted to continuously track the trajectory of the dynamic release of the child unmanned aerial vehicle by the parent unmanned aerial vehicle.

[0013] Further, the S1 comprises the following steps:

[0014] S11. Based on the release state of the parent-child unmanned aerial vehicle, the release state vector of the parent unmanned aerial vehicle with respect to the child unmanned aerial vehicle is set;

[0015] S12. Based on the release state vector of the parent unmanned aerial vehicle with respect to the child unmanned aerial vehicle, the time-varying parameters are calculated in real time, wherein the time-varying parameters include the total mass, the total center of mass and the total moment of inertia matrix;

[0016] The calculation expression of the time-varying parameters in the S12 is as follows:

[0017] , ,

[0018] ,

[0019] ,

[0020] ,

[0021] ,

[0022] wherein, denotes the total mass of the mother machine and all unreleased children at time t, denotes the mass of the mother machine, denotes the total number of children, denotes the release status vector of the mother machine with respect to the children at time t, denotes the mass of the child, denotes the child belonging to denotes the total center of mass position of the mother machine and all unreleased children at time t, denotes the center of mass position of the mother machine, denotes the center of mass position of the child, denotes the total moment of inertia of the mother machine and all unreleased children at time t, denotes the moment of inertia of the mother machine about its center of mass, denotes the skew-symmetric matrix of the translation vector of the mother machine, denotes the moment of inertia of the child about its center of mass, denotes the skew-symmetric matrix of the translation vector of the child, denotes the skew-symmetric matrix of the matrix, denotes the translation vector of the mother machine, denotes the translation vector of the child, wherein, when then the child is unreleased in the mother machine at time t, when then the child is released in the mother machine at time t;

[0023] S13, based on the center of mass shift caused by the mother machine releasing the children and the rotor thrust action, calculating the quasi-static disturbance moment and the transient impact disturbance according to the total center of mass in the time-varying parameter, wherein the transient impact disturbance includes the net recoil force and the net recoil torque;

[0024] The calculation expression of the quasi-static disturbance moment and the transient impact disturbance is as follows:

[0025] ,

[0026] ,

[0027] ,

[0028] wherein, denotes quasi-static disturbance moment, denotes total thrust generated by the rotor, denotes net recoil force on the total center of mass when releasing the sub-machine, denotes the release mechanism of the sub-machine on the mother machine, denotes the reverse impact force suffered by the mother machine when releasing the sub-machine through the first release mechanism, denotes net recoil moment, denotes the center of mass position of the first release mechanism;

[0029] S14, define the aerodynamic interference force and the aerodynamic interference moment according to the disturbed flow field at the moment of releasing the sub-machine;

[0030] S15, construct a composite disturbance force based on the net recoil force, the aerodynamic interference force and the external disturbance force in the mother machine body coordinate system ;

[0031] The calculation expression of the composite disturbance force is as follows:

[0032] ,

[0033] wherein, denotes the composite disturbance force, denotes the rotation matrix from the mother machine body coordinate system to the ground inertia coordinate system, denotes the aerodynamic interference force, denotes the external disturbance force;

[0034] S16, derive the time-varying inertia according to the Newton Euler equation to obtain the moment of the sudden impact of the moment of inertia;

[0035] S17, construct a composite disturbance moment based on the quasi-static disturbance moment, the net recoil moment, the aerodynamic interference moment and the sudden impact moment of the moment of inertia in the mother machine body coordinate system ;

[0036] The calculation expression of the composite disturbance moment is as follows:

[0037] ,

[0038] wherein, denotes the composite disturbance moment, denotes the aerodynamic interference moment, denotes the sudden impact moment of the moment of inertia;

[0039] S18, construct a time-varying dynamics model based on the composite disturbance force and the composite disturbance moment;

[0040] The calculation expression for the time-varying dynamic model is as follows:

[0041] ,

[0042] ,

[0043] in, This indicates the acceleration of the mother machine. This represents the total thrust generated by the rotor. Represents the gravitational acceleration vector. Indicates centripetal acceleration. This represents the angular acceleration of the mother machine. Represents the coordinate system of the mother machine. The three-axis control torque below, This represents the angular velocity of the mother machine. This represents the angular velocity matrix of the mother machine.

[0044] Further, step S2 includes the following steps:

[0045] S21. Based on the second-order integrator system of translational dynamics reconstruction, a third-order linear extended state observer is constructed, wherein the third-order linear extended state observer is used to predict the lumped disturbance acceleration in real time.

[0046] S22. Based on the first-order integrator system of rotational dynamics reconstruction, construct three parallel second-order linear extended state observers for attitude, wherein the three parallel second-order linear extended state observers for attitude are used to predict lumped rotational disturbances in real time.

[0047] The calculation expression for the attitude second-order linear extended state observer is as follows:

[0048] ,

[0049] in, Indicates the first Predicted angular acceleration values ​​for each channel. Indicates the first Predicted values ​​of lumped disturbances for each channel. This represents the bandwidth of the attitude second-order linearly extended state observer. Indicates the first The angular velocity of each channel, Indicates the first Predicted values ​​of angular velocity for each channel. Indicates the first The rate of change of the predicted value of the lumped disturbance for each channel, where, =1, 2, 3.

[0050] Further, step S3 includes the following steps:

[0051] S31. Based on the time-varying dynamic model, the translational dynamic model is decoupled with acceleration as the core to obtain the decoupled translational dynamic model of the position controller.

[0052] The calculation expression for the decoupled translational dynamics model of the position controller is as follows:

[0053] ,

[0054] ,

[0055] ,

[0056] ,

[0057] in, Indicates thrust acceleration. Indicates a known acceleration. Represents the lumped disturbance acceleration. It is defined as;

[0058] S32. Calculate the desired speed reference command based on the position error and integral error according to the outer position loop;

[0059] The calculation expression for the desired speed reference command is as follows:

[0060] ,

[0061] in, Indicates positional error. Indicates the mother machine in the ground inertial coordinate system The lower position, Indicates the trajectory position command. Indicates the integral error. express Position error at any given time This represents the result of the time derivative. Indicates the desired speed reference command. The adjustment factor representing the positive definite diagonal gain at position. Indicates the mother machine in the ground inertial coordinate system The speed of the drop This represents the position positive definite diagonal gain matrix. This represents the integral positive definite diagonal gain matrix;

[0062] S33. Based on the desired speed reference command, use the speed loop to calculate the ideal feedback acceleration based on the speed tracking error;

[0063] The calculation expression of the ideal feedback acceleration is as follows:

[0064] ,

[0065] ,

[0066] wherein, represents the ideal feedback acceleration, represents the speed error dynamic, represents the speed positive diagonal gain matrix, represents the speed error;

[0067] S34, calculating the expected thrust acceleration based on the decoupling dynamics model of the position controller according to the nonlinear dynamic inverse and the ideal feedback acceleration;

[0068] The calculation expression of the expected thrust acceleration is as follows:

[0069] ,

[0070] wherein, represents the expected thrust acceleration, represents the lumped disturbance acceleration prediction value;

[0071] S35, calculating the expected thrust vector according to the expected thrust acceleration and the real-time total mass in the time-varying parameters;

[0072] The calculation expression of the expected thrust vector is as follows:

[0073] ,

[0074] wherein, represents the expected thrust vector;

[0075] S36, decomposing the expected thrust vector to obtain the expected total thrust size and the expected attitude;

[0076] The calculation expressions of the expected total thrust amplitude and the expected attitude are as follows, respectively:

[0077] ,

[0078] ,

[0079] wherein, represents the expected total thrust size, represents the expected total thrust amplitude, represents the expected attitude.

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

[0081] S41, the command angular velocity is calculated by differentiating the time-varying desired attitude, and the command angular acceleration is calculated by filtering and differentiating the command angular velocity;

[0082] S42, the error signal is defined, wherein the error signal includes the attitude error and the angular velocity tracking error;

[0083] S43, the target angular acceleration is constructed based on the command angular acceleration and the error signal;

[0084] The calculation expression of the target angular acceleration is as follows:

[0085] ,

[0086] Wherein, represents the target angular acceleration, represents the command angular acceleration, represents the angular acceleration gain matrix, represents the angular velocity tracking error, represents the adjustment factor of the attitude gain, represents the attitude gain matrix, represents the attitude error;

[0087] S44, the controller input angular acceleration of each channel is calculated based on the three attitude second-order linear extended state observers in parallel according to the target angular acceleration;

[0088] The calculation expression of the controller input angular acceleration of each channel is as follows:

[0089] ,

[0090] Wherein, represents the controller input angular acceleration of the th channel, represents the target desired angular acceleration of the th channel;

[0091] S45, the control torque is calculated based on the nonlinear dynamic inverse and the rotational dynamics model according to the controller input angular acceleration of each channel, the target angular acceleration and the time-varying dynamics model;

[0092] The calculation expression of the control torque is as follows:

[0093] ,

[0094] Wherein, represents the predicted value of the lumped disturbance, represents the known disturbance torque;

[0095] Further, the S5 comprises the following steps:

[0096] S51, constructing a total cost function according to the total thrust size and the control moment;

[0097] The calculation expression of the total cost function is as follows:

[0098] ,

[0099] ,

[0100] ,

[0101] ,

[0102] ,

[0103] wherein, represents the total cost of the particle , represents the instantaneous performance cost, represents the instantaneous energy consumption cost, represents the end time of the release control of the mother machine, represents the start time of the release control of the mother machine, represents the time differential, represents the position-based weight, represents the th axial direction, represents the th axial direction, represents the th axial direction, represents the th axial direction, represents the axial weight of the th axial direction, represents the absolute value of the axial position error of the th axial direction, represents the attitude weight, represents the th attitude angle, represents the roll angle, represents the pitch angle, represents the yaw angle, represents the attitude weight of the th attitude angle, represents the absolute value of the attitude error of the th attitude angle, represents the global energy consumption weight, represents the thrust weight, represents the total thrust, represents the an axial control moment is de-weighted, a first axial control moment is represented;

[0104] S52, constructing candidate solution vectors of the trajectory tracking controller based on controller parameters of the trajectory tracking controller according to physical symmetry of the mother-daughter type unmanned aerial vehicle in the X-Y plane;

[0105] S53, taking each candidate solution vector of the trajectory tracking controller as a particle respectively;

[0106] S54, continuously iterating with a total cost function as a target function according to a particle swarm optimization algorithm to obtain an optimal solution vector of a real-time trajectory tracking controller;

[0107] S55, taking controller parameters corresponding to the optimal solution vector of the real-time trajectory tracking controller as real-time target controller parameters.

[0108] Further, the S54 comprises the following steps:

[0109] S541, initializing a particle swarm and a current iteration number as 0 based on a parameter space of the candidate solution of the trajectory tracking controller according to the particle swarm optimization algorithm;

[0110] S542, executing a mother aircraft release control program based on a position and a speed of each particle in the particle swarm respectively, and extracting three axial position errors, three attitude angle attitude errors, total thrust and three axial control moments as simulation outputs of the particle;

[0111] S543, evaluating a total cost of each particle in the particle swarm based on the total cost function according to the simulation outputs of each particle;

[0112] S544, updating an individual historical optimal value and a global historical optimal value in the particle swarm according to the total cost of each particle;

[0113] S545, updating a speed and a position of each particle according to the individual historical optimal value and the global historical optimal value in the particle swarm;

[0114] S546, judging whether the current iteration number reaches an iteration number threshold, if yes, entering S547, otherwise, adding 1 to the current particle iteration number and returning to S542;

[0115] S547, taking a candidate solution vector corresponding to the global historical optimal value as an optimal solution vector of the trajectory tracking controller.

[0116] The beneficial effects of the present application are: the trajectory tracking control method for the parent-child unmanned aerial vehicle air release provided by the present application introduces nonlinear dynamic inverse NDI control, real-time obtains time-varying parameters, the nonlinear dynamic inverse controller utilizes these accurate time-varying model information, actively and instantly decouples and feedforward compensates gravity acceleration, centripetal acceleration, gyro coupling moment and center of mass offset moment and the like in control law derivation, through feedforward combining the active compensation mechanism of nonlinear dynamic inverse, the prediction error and delay of the extended state observer in dealing with known steps are avoided, and the trajectory smoothness and precision of the system in the release transient process are significantly improved; the present application utilizes the double-channel linear extended state observer of position and attitude, focuses on the passive suppression of these unmodeled transient dynamics, unifies all residual disturbances as lumped disturbances, and performs real-time prediction and compensation, the high bandwidth characteristic ensures the rapid capture and attenuation of instantaneous impact, even in the case of serious model mismatch, the linear extended state observer can still accurately identify and compensate the steady-state gravity disturbance in real time, while the position error is controlled within centimeters, and the excellent robustness of the trajectory tracking controller under strong uncertainty is ensured; the composite trajectory tracking control scheme provided by the present application contains up to 16 highly coupled inner loop, outer loop gain and ESO bandwidth and other key parameters, and through the introduction of the particle swarm optimization algorithm, the position-attitude joint optimization strategy is adopted, the parameter setting problem is converted into an optimization problem with the total cost function as the target, and the total cost function balances the trajectory tracking performance and control energy consumption, through the global optimization ability of the particle swarm optimization algorithm, the target controller parameters are successfully converged in the 16-dimensional space, which not only breaks through the parameter setting bottleneck, but also ensures that the trajectory tracking control scheme can achieve the optimization of performance indicators in complex tasks.

[0117] On the other hand, the present application also provides a trajectory tracking control system based on the trajectory tracking control method for the parent-child unmanned aerial vehicle air release, comprising:

[0118] The parameter updating module is configured to calculate the time-varying parameters in real time based on the release state of the parent-child unmanned aerial vehicle, and construct a time-varying dynamics model;

[0119] The channel extended observer module is configured to construct a position three-order linear extended state observer and three attitude two-order linear extended state observers in parallel in the channel respectively;

[0120] The position outer loop module is configured to perform composite control law derivation and thrust calculation based on the time-varying dynamics model and the position three-order linear extended state observer, and obtain the total thrust size and the expected attitude;

[0121] An attitude inner loop module is configured to calculate an instruction angular acceleration according to a desired attitude and define an error signal, and calculate a control torque based on three attitude second-order linear extended state observers, a nonlinear dynamic inverse and a time-varying dynamics model in parallel channels.

[0122] A parameter optimization module is configured to construct a total cost function according to a total thrust size and the control torque, and iteratively optimize a candidate solution vector constructed by controller parameters of the trajectory tracking controller based on a particle swarm optimization algorithm to obtain real-time target controller parameters.

[0123] A trajectory tracking module is configured to dynamically adjust the trajectory tracking controller based on the real-time target controller parameters to continuously track a trajectory of the sub-mother unmanned aerial vehicle during a dynamic release process of the sub unmanned aerial vehicle.

[0124] The trajectory tracking control system based on the trajectory tracking control method for the aerial release of the sub-mother unmanned aerial vehicle provided by the application is a system corresponding to the trajectory tracking control method for the aerial release of the sub-mother unmanned aerial vehicle, and is used to realize continuous and accurate trajectory tracking control of the sub-mother unmanned aerial vehicle under various working conditions including the aerial release of the sub unmanned aerial vehicle based on the trajectory tracking control method for the aerial release of the sub-mother unmanned aerial vehicle.

[0125] Other advantages of the application will be analyzed in more detail in the following embodiments. BRIEF DESCRIPTION OF DRAWINGS

[0126] In order to more clearly illustrate the technical solutions of the embodiments of the application, the drawings needed in the embodiments will be briefly introduced as follows. It should be understood that the following drawings only show some embodiments of the application, and therefore should not be regarded as a limitation on the scope. Other related drawings can also be obtained by those skilled in the art without creative labor on the basis of these drawings.

[0127] Figure 1 The step flow chart of the trajectory tracking control method for the aerial release of the sub-mother unmanned aerial vehicle in the embodiments of the application.

[0128] Figure 2 The block diagram of the trajectory tracking control system based on the trajectory tracking control method for the aerial release of the sub-mother unmanned aerial vehicle in the embodiments of the application. DETAILED DESCRIPTION

[0129] The technical solutions in the embodiments of the present application will be clearly and completely described below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all the embodiments. The components of the embodiments of the present application described and shown in the drawings can be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present application provided in the drawings is not intended to limit the scope of the claimed present application, but only represents selected embodiments of the present application. Based on the embodiments of the present application, all other embodiments obtained by those skilled in the art without creative work belong to the scope of protection of the present application.

[0130] In the present solution, in order to accurately describe the motion of the mother unmanned aerial vehicle in the three-dimensional space in the mother-daughter unmanned aerial vehicle, two coordinate systems are defined, which are the ground inertial coordinate system and the mother machine body coordinate system .

[0131] The ground inertial coordinate system is fixed on the ground to describe the global position and trajectory of the mother-daughter unmanned aerial vehicle, and the North-Earth-East (NED) convention is adopted, wherein, is a preset ground fixed position, the axis points to the north, the axis points to the east, and the axis points to the center of the earth.

[0132] The mother machine body coordinate system is fixed on the mother machine body, and the front-right-down convention is adopted, wherein, is located at the geometric center of the mother machine, the axis points to the nose direction of the mother machine, the axis points to the right side of the mother machine, and the axis points to the lower side of the mother machine; the angular velocity and control moment of the mother machine are defined in the mother machine body coordinate system.

[0133] In the present solution, the mother machine can be regarded as a standard six-degree-of-freedom (6-DOF) rigid body when the daughter machine is mounted and released, and based on the dynamic model of the mother machine, the state vector of the mother machine is defined as:

[0134] ,

[0135] wherein, represents the position of the mother machine in the ground inertial coordinate system , represents the velocity of the mother machine in the ground inertial coordinate system , represents the Euler angle, and represents the angular velocity of the mother machine, wherein the Euler angle includes the roll angle pitch angle and yaw angle ;

[0136] The dynamics model of the parent machine includes a translation kinematics part and a rotation dynamics part, and a calculation expression of the dynamics model of the parent machine is as follows:

[0137] ,

[0138] ,

[0139] wherein, denotes a derivative of a position of the parent machine in a ground inertial coordinate system , denotes a conversion matrix from an angular velocity of the parent machine in a parent machine body coordinate system to a rate of change of Euler angle;

[0140] According to the dynamics model of the parent machine, a dynamics equation of the parent machine can be constructed based on a Newton-Euler method:

[0141] ,

[0142] ,

[0143] wherein, denotes an acceleration of the parent machine in the ground inertial coordinate system , denotes a mass of the parent machine, denotes a rotation matrix from the parent machine body coordinate system to the ground inertial coordinate system, denotes a total thrust generated by the rotor, denotes a gravity acceleration vector, denotes an external disturbance force, denotes an angular acceleration of the parent machine, denotes an inverse matrix of a moment of inertia matrix of the parent machine, denotes a control moment generated by the rotor, denotes the moment of inertia matrix of the parent machine;

[0144] As Figure 1 shown, in one aspect, in an embodiment of the present application, the present application provides a trajectory tracking control method for air release of a parent-child unmanned aerial vehicle, comprising the following steps:

[0145] S1, based on a release state of the parent-child unmanned aerial vehicle, a time-varying parameter is calculated in real time, and a time-varying dynamics model is constructed;

[0146] This scheme establishes a six-degree-of-freedom (6-DOF) nonlinear time-varying aerodynamic model that accurately describes the entire process of mother aircraft release in a mother-daughter UAV, and updates the system physical properties required for control in real time.

[0147] S1 includes the following steps:

[0148] S11. Based on the release state of the mother drone, set the release state vector of the mother drone with respect to the daughter drone;

[0149] In this scheme, the release status of the mother drone in a mother-daughter drone system can reflect whether the daughter drones carried by the mother drone have been released.

[0150] S12. Based on the release state vector of the mother machine with respect to the daughter machine, the time-varying parameters are calculated in real time, including the total mass, total center of mass and total moment of inertia matrix.

[0151] The calculation expression for the time-varying parameter in S12 is as follows:

[0152] , ,

[0153] ,

[0154] ,

[0155] ,

[0156] ,

[0157] in, express The total mass of the mother machine and all unreleased daughter machines at that moment. Indicates the mass of the mother machine. This indicates the total number of slave machines. express The mother machine of the moment about the first Release state vector of each submachine Indicates the first The quality of the individual machine Indicates belonging to, express The position of the center of mass of the mother machine and all unreleased daughter machines at any given moment. Indicates the position of the mother machine's center of gravity. Indicates the first The position of the centroid of the machine. express The total moment of inertia of the mother machine and all unreleased slave machines at any given moment. This represents the moment of inertia of the mother machine about its own center of mass. denotes the skew-symmetric matrix of the translational vector of the parent, denotes the rotational inertia of the th sub around its own mass center, denotes the skew-symmetric matrix of the translational vector of the th sub, denotes the skew-symmetric matrix of the matrix, denotes the translational vector of the parent, denotes the translational vector of the th sub, where when then at the moment the th sub in the parent is not released, when then at the moment the th sub in the parent is released;

[0158] In the scheme, the total mass in the time-varying parameter is defined as the sum of the masses of the parent and the unreleased sub; the instantaneous total mass center of the parent and the sub reflects the rotational dynamics of the whole sub-parent unmanned aerial vehicle, and the scheme takes the weighted average position of the parent and all unreleased subs in the parent body coordinate system under as the total mass center; in the scheme, according to the parallel axis theorem and the rigid body superposition principle, the total rotational inertia is obtained around the instantaneous total mass center, and the calculation of the total rotational inertia comprehensively considers the contribution of the parent itself and the contribution of all unreleased subs; since the mass center of the sub is offset relative to the total mass center of the sub-parent unmanned aerial vehicle, the cross inertia term introduced causes the total rotational inertia to become a non-diagonal, dense and time-varying inertia matrix, wherein the real-time update of the total rotational inertia is the basis for the attitude loop nonlinear dynamic inverse (NDI) compensation of the gyro coupling moment;

[0159] S13, according to the total mass center in the time-varying parameter, based on the mass center offset caused by the parent releasing the sub and the rotor thrust action, the quasi-static disturbance moment and the transient impact disturbance are calculated, wherein the transient impact disturbance includes the net recoil force and the net recoil moment;

[0160] The calculation expression of the quasi-static disturbance moment and the transient impact disturbance is as follows:

[0161] ,

[0162] ,

[0163] ,

[0164] wherein, denotes the quasi-static disturbance moment, denotes the total thrust generated by the rotor, denotes the net recoil force to the total mass center when the sub is released, The release mechanism of the child machine on the parent machine, The reverse impact force when the parent machine releases the child machine through the first release mechanism, The net recoil torque, The centroid position of the first release mechanism;

[0165] In the scheme, the quasi-static disturbance torque is the torque generated by the combined action of the centroid offset and the rotor thrust. After the child machine is released, the total centroid of the mother-son unmanned aerial vehicle no longer coincides with the geometric center of the machine body, and the eccentricity generated forms a force arm, thereby generating a persistent disturbance torque proportional to the current thrust, i.e., the quasi-static disturbance torque.

[0166] S14, according to the disturbed flow field at the moment of release of the child machine, define the aerodynamic disturbance force and the aerodynamic disturbance torque;

[0167] In the scheme, the sudden separation of the child machine disturbs the flow field and generates complex aerodynamic disturbance force and aerodynamic disturbance torque. Since the flow field induced by the child machine at the moment of separation is extremely complex and is strongly coupled with the flight state, the scheme regards this disturbance as a bounded unknown disturbance, which can be suppressed by the controller.

[0168] S15, based on the net recoil force, the aerodynamic disturbance force and the external disturbance force in the parent machine body coordinate system , construct a composite disturbance force;

[0169] The calculation expression of the composite disturbance force is as follows:

[0170]

[0171] Among them, The composite disturbance force, The rotation matrix from the parent machine body coordinate system to the ground inertial coordinate system, The aerodynamic disturbance force, The external disturbance force; in the scheme, the external disturbance force is the disturbance force such as wind disturbance which is not directly modeled;

[0172] S16, derive the time-varying inertia according to Newton Euler equation, get the moment of inertia sudden impact torque;

[0173] S17, based on the quasi-static disturbance torque, the net recoil torque, the aerodynamic disturbance torque and the moment of inertia sudden impact torque in the parent machine body coordinate system , construct a composite disturbance torque;

[0174] The calculation expression of the composite disturbance torque is as follows:

[0175] ​,

[0176] wherein, represents a compound disturbance torque, represents an aerodynamic interference torque, represents a rotational inertia mutation impact torque;

[0177] S18, constructing a time-varying dynamics model based on the compound disturbance force and the compound disturbance torque;

[0178] The calculation expression of the time-varying dynamics model is as follows:

[0179] ,

[0180] ,

[0181] wherein, represents an acceleration of the parent machine, represents a total thrust generated by the rotor, represents a gravity acceleration vector, represents a centripetal acceleration, represents an angular acceleration of the parent machine, represents three-axis control torques in a parent machine body coordinate system represents an angular velocity of the parent machine, represents an angular velocity matrix of the parent machine. In the scheme, for constructing the time-varying dynamics model, a robust controller is designed to calculate the total thrust and the three-axis control torques, so that the parent machine can accurately track the given desired trajectory in the whole process of the task and quickly suppress various compound disturbances at the moment of release, thereby ensuring the smooth, safe and high-precision flight of the parent-child unmanned aerial vehicle.

[0182] S2, respectively constructing a position third-order linear extended state observer and three channel-parallel attitude second-order linear extended state observers;

[0183] In the scheme, through the linear extended state observers of the position and attitude double channels, all remaining position dynamics and external disturbances, i.e. lumped disturbances, which are difficult to accurately model, are predicted and compensated in real time; the lumped disturbance is expanded into a new state variable in the scheme, so that the complex position disturbance is converted into a prediction problem of the observable state; the observer gain of the extended state observer is determined by the bandwidth parameterization method.

[0184] The S2 includes the following steps:

[0185]

[0186] ​S21. Based on the second-order integrator system of translational dynamics reconstruction, a third-order linear extended state observer is constructed, wherein the third-order linear extended state observer is used to predict the lumped disturbance acceleration in real time.

[0187] In this scheme, the lumped disturbance acceleration includes all unmodeled and unpredictable translational disturbance terms;

[0188] In this scheme, the position third-order linear extended state observer is in axis, shaft and The axes can be completely decoupled, and the corresponding calculation expressions have the same structure in each axis direction. In this embodiment, we take... Taking the shaft as an example, then The calculation expression for the third-order linear extended state observer in the axial direction is as follows:

[0189] ,

[0190] in, This indicates that the mother machine and all unreleased slave machines are in The derivative of the predicted position along the axis with respect to time. This indicates that the mother machine and all unreleased slave machines are in Predicted value of axial velocity. This represents the bandwidth of the third-order linearly extended state observer at the location. This indicates that the mother machine and all unreleased slave machines are in The actual position in the axial direction, This indicates that the mother machine and all unreleased slave machines are in Predicted value of axial position. This indicates that the mother machine and all unreleased slave machines are in The derivative of the predicted axial velocity with respect to time. This indicates that the mother machine and all unreleased slave machines are in Predicted values ​​of lumped disturbance acceleration in the axial direction. This indicates that the mother machine and all unreleased slave machines are in Thrust acceleration in the axial direction, This indicates that the mother machine and all unreleased slave machines are in Known acceleration in the axial direction, Indicates the observer bandwidth. This indicates that the mother machine and all unreleased slave machines are in The time derivative of the predicted disturbance value in the axial direction;

[0191] In the scheme, the thrust acceleration and the known acceleration are control items, the thrust acceleration is calculated by the controller, the known acceleration is a nonlinear dynamic inverse (NDI) compensation item, and the known acceleration is the sum of the gravitational acceleration of the parent aircraft and the unreleased sub-machine and the centripetal acceleration caused by the total mass center offset;

[0192] S22, based on the first-order integrator system reconstructed by rotational dynamics, three channel-parallel attitude second-order linear extended state observers are constructed, wherein the three channel-parallel attitude second-order linear extended state observers are used to predict the lumped rotational disturbance in real time;

[0193] In the scheme, the three channel-parallel attitude second-order linear extended state observers are set for the attitude angles of the three orientations corresponding to the attitude; the predicted value of the lumped rotational disturbance contains all the remaining dynamics that are not compensated by the nonlinear dynamic inverse (NDI), mainly including: external disturbance torque and internal dynamic transient term, wherein the external disturbance torque is the aerodynamic disturbance torque and the mechanical recoil torque, and the internal dynamic transient term is the variable inertia impact torque, wherein the mechanical recoil torque is the net recoil torque;

[0194] The calculation expression of the attitude second-order linear extended state observer is as follows:

[0195] ,

[0196] Wherein, represents the angular acceleration prediction value of the first channel, represents the predicted value of the lumped disturbance of the first channel, represents the bandwidth of the attitude second-order linear extended state observer, represents the angular velocity of the first channel, represents the predicted value of the angular velocity of the first channel, represents the rate of change of the predicted value of the lumped disturbance of the first channel, wherein =1, 2, 3.

[0197] S3, based on the time-varying dynamics model and the position third-order linear extended state observer, a compound control law derivation and thrust calculation is performed to obtain the total thrust size and the expected attitude;

[0198] The scheme proposes a trajectory tracking controller based on nonlinear dynamic inverse (NDI), linear extended state observer (LESO) and cascade feedback, which adopts the cascade structure of position outer loop and attitude inner loop, estimates and compensates the concentrated disturbance in real time through the linear extended state observer, so as to realize high-precision robust tracking; in the scheme, the parameter time variation is accurately compensated through the active feedforward characteristic of the nonlinear dynamic inverse, and the composite disturbance is estimated and suppressed in real time through the observation ability of the linear extended state observer;

[0199] The S3 comprises the following steps:

[0200] S31, based on the time-varying dynamics model, decoupling the translational dynamics model with acceleration as the core to obtain the decoupling translational dynamics model of the position controller; in the scheme, according to the time-varying dynamics model, the calculation expression of the translational dynamics model can be obtained as: ;

[0201] The calculation expression of the decoupling translational dynamics model of the position controller is as follows:

[0202] ,

[0203] ,

[0204] ,

[0205] ,

[0206] Among them, represents the thrust acceleration, represents the known acceleration, represents the lumped disturbance acceleration, represents the definition as;

[0207] In the scheme, the thrust acceleration is a control item, which is calculated by the composite trajectory tracking controller; the known acceleration is a nonlinear dynamic inverse compensation item, which includes the known gravity acceleration and the real-time calculation according to the current aircraft angular velocity and total mass center; the lumped disturbance acceleration is a prediction item of the extended state observer, which contains all unmodeled and difficult to predict disturbances, and is the object to be identified and supplemented by the extended state observer.

[0208] S32, calculating the expected speed reference command according to the position error and integral error based on the position outer loop;

[0209] The calculation expression of the expected speed reference command is as follows:

[0210] ,

[0211] Among them, position error, position of the mother machine in ground inertial coordinate system , trajectory position command, integral error, position error, at the moment, time differential result, expected speed reference command, adjusting factor of positive definite diagonal gain of position, speed of the mother machine in ground inertial coordinate system , positive definite diagonal gain matrix of position, positive definite diagonal gain matrix of integral;

[0212] S33, according to the expected speed reference command, the ideal feedback acceleration is calculated by the speed loop according to the speed tracking error;

[0213] The calculation expression of the ideal feedback acceleration is as follows:

[0214] ,

[0215] ,

[0216] wherein, ideal feedback acceleration, speed error dynamic, positive definite diagonal gain matrix of speed, speed error;

[0217] S34, according to the nonlinear dynamic inverse and the ideal feedback acceleration, the expected thrust acceleration is calculated based on the decoupling dynamics model of the position controller;

[0218] In the scheme, the actual mother machine acceleration accurately tracks the ideal acceleration command issued by the feedback controller, and the control term, i.e. the thrust acceleration is obtained by inverse solution based on the decoupling dynamics model, and the predicted concentrated disturbance acceleration obtained by the position three-order linear extended state observer is used to replace the unmeasurable concentrated disturbance acceleration, so as to obtain the expected thrust acceleration;

[0219] The calculation expression of the expected thrust acceleration is as follows:

[0220] ,

[0221] wherein, expected thrust acceleration, a represents the predicted value of the collective disturbance acceleration;

[0222] S35, calculating the desired thrust vector according to the desired thrust acceleration and the real-time total mass in the time-varying parameter;

[0223] The calculation expression of the desired thrust vector is as follows:

[0224] ,

[0225] , a represents the desired thrust vector;

[0226] S36, decomposing the desired thrust vector to obtain the desired total thrust magnitude and the desired attitude;

[0227] The calculation expressions of the desired total thrust magnitude and the desired attitude are as follows:

[0228] ,

[0229] ,

[0230] , a represents the desired total thrust magnitude, a represents the desired total thrust magnitude, a represents the desired attitude.

[0231] S4, calculating the command angular acceleration according to the desired attitude, defining an error signal, and calculating the control torque based on three attitude second-order linear extended state observers, a nonlinear dynamic inverse, and a time-varying dynamic model in parallel channels;

[0232] The scheme quickly and accurately tracks the desired attitude calculated by the position outer loop by designing a compound control rate of the attitude inner loop;

[0233] The S4 includes the following steps:

[0234] S41, calculating the command angular velocity by differentiating the desired attitude changing with time, and calculating the command angular acceleration by filtering and differentiating the command angular velocity; in the scheme, a low-pass filter or trajectory planning is used to generate a smooth command angular acceleration to avoid numerical noise;

[0235] S42, defining an error signal, wherein the error signal includes an attitude error and an angular velocity tracking error;

[0236] In the scheme, in the case of representing a unit quaternion by a rigid body attitude, the attitude error can be extracted by the vector part of the error quaternion, and the error quaternion can be obtained by the given desired attitude quaternion and the current attitude quaternion, i.e. , Represents the error quaternion. Represents the expected pose quaternion. To represent quaternion multiplication, The quaternion represents the conjugate of the current attitude quaternion; the attitude error can accurately characterize the proportional relationship between the minimum rotation axis and the amount of rotation required to rotate from the current attitude to the desired attitude; if the error quaternion is represented as ,in, Indicates the rotation angle. If the unit rotation axis is represented, then the attitude error... When the attitude error is small, that is hour, It is approximately linearly related to the rotation vector, which facilitates controller design.

[0237] The expression for calculating angular velocity tracking error is: ,in, Indicates angular velocity tracking error. This indicates the commanded angular velocity; in this scheme, the angular velocity tracking control error directly reflects the dynamic tracking performance.

[0238] S43. Construct the target angular acceleration based on the command angular acceleration and error signal;

[0239] The expression for calculating the target angular acceleration is as follows:

[0240] ,

[0241] in, Indicates the target angular acceleration. Indicates the command angular acceleration. Represents the angular acceleration gain matrix. Indicates angular velocity tracking error. The adjustment factor representing the attitude gain. Represents the attitude gain matrix. Indicates attitude error;

[0242] S44. Based on the target angular acceleration, the controller input angular acceleration of each channel is calculated using three parallel second-order linear extended state observers.

[0243] The calculation expressions for the controller input angular acceleration of each channel are as follows:

[0244] ,

[0245] in, Indicates the first The controller input angular acceleration for each channel, Indicates the first a target desired angular acceleration of each channel;

[0246] S45, according to the controller input angular acceleration, target angular acceleration and time-varying dynamic model of each channel, the control torque is calculated based on the nonlinear dynamic inverse and the rotational dynamic model;

[0247] In the scheme, according to the time-varying dynamic model, the calculation expression of the translational dynamic model is obtained as: ;

[0248] The calculation expression of the control torque is as follows:

[0249] ,

[0250] Wherein, represents the predicted value of the lumped disturbance, represents the known disturbance torque;

[0251] In the scheme, As the desired feedforward, it compensates for the expected angular acceleration of the collection; As a proportional-differential feedback, it eliminates the attitude error and angular velocity error; As a known disturbance feedforward, it actively cancels the modelable disturbance torque; As ESO compensation, it is the key anti-disturbance part to estimate and suppress all unknown disturbance torques; the predicted value of the lumped disturbance is the predicted value of the unknown disturbance by the three attitude second-order linear extended state observers in parallel; the known disturbance torque includes all torques that can be calculated in real time according to the current state and parameter updates, mainly including gyro coupling torque and quasi-static disturbance torque generated by the center of mass offset; the unknown disturbance torque includes all disturbance torques that are difficult to accurately model, such as net recoil torque, aerodynamic disturbance torque and rotational inertia sudden impact torque;

[0252] In the scheme, according to the above compound control law, the attitude loop is dynamically linearized into a proportional-differential system disturbed by a bounded estimation error.

[0253] S5, according to the total thrust size and the control torque, a total cost function is constructed, and based on the particle swarm optimization algorithm, the candidate solution vector constructed by the controller parameters of the trajectory tracking controller is iteratively optimized to obtain real-time target controller parameters;

[0254] The scheme converts the controller parameter setting into a multi-dimensional, constrained nonlinear optimization problem, and introduces the particle swarm optimization algorithm for automatic optimization; to solve the limitations of the traditional inner-outer loop decoupling optimization strategy, the scheme adopts a position-attitude joint optimization strategy.

[0255] The S5 comprises the following steps:

[0256] S51, constructing a total cost function according to the total thrust size and the control moment;

[0257] The cost function designed in the scheme aims to balance the trajectory tracking performance and the control energy consumption, and can quantify multiple-dimensional performance indicators such as tracking error, overshoot, energy consumption, etc. into a single scalar cost value;

[0258] The calculation expression of the total cost function is as follows:

[0259] ,

[0260] ,

[0261] ,

[0262] ,

[0263] ,

[0264] wherein, represents the total cost of the particle , represents the instantaneous performance cost, represents the instantaneous energy consumption cost, represents the end time of the release control of the mother machine, represents the start time of the release control of the mother machine, represents the time differential, represents the position-based weight, represents the first axial direction, represents the axial direction, represents the axial direction, represents the axial direction, represents the axial weight of the first axial direction, represents the absolute value of the axial position error of the first axial direction, represents the attitude weight, represents the first attitude angle, represents the roll angle, represents the pitch angle, represents the yaw angle, represents the attitude weight of the first attitude angle, represents the absolute value of the attitude error of the first attitude angle. represents a global energy weight, represents a thrust weight, represents a total thrust, represents a control moment de-weight of the axis, represents a control moment of the axis;

[0265] In the scheme, the instantaneous performance cost is used to punish the tracking error of the controller on the position and the attitude; the attitude weight is used to balance the dimension to balance the contribution of the position error and the attitude error in the same cost function; the instantaneous energy cost is used to punish the excessive control output, which embodies the consideration of the system energy consumption and the actuator loss, wherein the actuator includes a motor and other controllable components;

[0266] S52, according to the physical symmetry of the mother-child type unmanned aerial vehicle in the X-Y plane, a candidate solution vector of the trajectory tracking controller is constructed based on the controller parameters of the trajectory tracking controller;

[0267] In the scheme, according to the physical symmetry of the mother-child type unmanned aerial vehicle in the X-Y plane, the same type controller parameters of the X axis and the Y axis in the position outer loop and the attitude inner loop are combined to be given the same value in the optimization process, while the Z axis parameters are optimized independently; In the scheme, the candidate solution vector of the trajectory tracking controller is a 16-dimensional vector composed of 16 to-be-optimized controller parameters, as shown in Table 1:

[0268] Table 1 Controller parameter table

[0269]

[0270] In the scheme, , , , , , and have corresponding values on the axis, axis and axis, since the axis and the axis have the same symmetry, they are combined into the same value, and the Z axis has an independent value; and are scalar adjustment factors of the gain, and only have one value;

[0271] S53, each candidate solution vector of the trajectory tracking controller is taken as a particle ;

[0272] S54, continuously iterating with the total cost function as the objective function according to the particle swarm optimization algorithm to obtain an optimal solution vector of the real-time trajectory tracking controller;

[0273] The S54 includes the following steps:

[0274] S541, initializing the particle swarm and the current iteration number as 0 based on the parameter space of the candidate solution of the trajectory tracking controller according to the particle swarm algorithm;

[0275] In the scheme, each potential solution of the trajectory tracking controller is regarded as a particle, and each particle has two core attributes in the search space, namely position and speed, wherein the position represents a candidate solution in the search space, that is, a set of controller parameters of the trajectory tracking controller to be optimized, and the speed represents the change direction and rate of the particle in the search space; each particle dynamically adjusts its flight according to its own experience and the experience of other particles in the group during the search process, and the adjustment depends on two key optimal values: individual historical optimal value and global historical optimal value, wherein the individual historical optimal value refers to the best position experienced by each particle itself, and the global historical optimal value refers to the best position discovered by the whole particle group.

[0276] S542, based on the position and speed of each particle in the particle swarm, respectively executing the mother machine release control program, and extracting 3 axial position errors, 3 attitude angle attitude errors, total thrust and 3 axial control moments as simulation outputs of the particle;

[0277] S543, based on the simulation output of each particle, evaluating the total cost of each particle in the particle swarm based on the total cost function;

[0278] S544, updating the individual historical optimal value and the global historical optimal value in the particle swarm according to the total cost of each particle;

[0279] S545, updating the speed and position of each particle according to the individual historical optimal value and the global historical optimal value in the particle swarm;

[0280] In the scheme, the speed and position of each particle are updated through a position and speed update model;

[0281] The calculation expression of the position and speed update model is as follows:

[0282] ,

[0283] ,

[0284] wherein, represents the updated i-th particle in the particle swarm, and position of the particle, indicates the current first position of the particle, indicates the updated first velocity of the particle, indicates the inertia weight, indicates the current first velocity of the particle, indicates the individual historical optimal value learning factor, indicates the individual search random coefficient, indicates the current first individual historical optimal value of the particle, indicates the global historical optimal value learning factor, indicates the global search random coefficient, indicates the global historical optimal value;

[0285] S546, determine whether the current iteration number reaches the iteration number threshold, if yes, go to S547, otherwise, add 1 to the current particle iteration number, and return to S542;

[0286] S547, the candidate solution vector corresponding to the global historical optimal value is taken as the optimal solution vector of the trajectory tracking controller.

[0287] S55, the controller parameter corresponding to the optimal solution vector of the real-time trajectory tracking controller is taken as the real-time target controller parameter.

[0288] S6, dynamically adjust the trajectory tracking controller based on the real-time target controller parameter, to continuously track the trajectory of the dynamic release of the child drone by the parent drone.

[0289] As shown in Figure 2 , in another aspect, the present application also provides a trajectory tracking control system based on the above trajectory tracking control method for the aerial release of the parent-child unmanned aerial vehicle, comprising:

[0290] a parameter updating module, configured to calculate the time-varying parameter in real time based on the release state of the parent-child unmanned aerial vehicle, and construct a time-varying dynamics model;

[0291] a channel expansion observer module, configured to construct a position three-order linear expansion state observer, and three attitude two-order linear expansion state observers in parallel with the channel respectively;

[0292] a position outer loop module, configured to derive a compound control law and calculate thrust based on the time-varying dynamics model and the position three-order linear expansion state observer, to obtain the total thrust size and the expected attitude;

[0293] An attitude inner loop module is configured to calculate an instruction angular acceleration according to a desired attitude and define an error signal, and calculate a control torque based on three attitude second-order linear extended state observers, a nonlinear dynamic inverse and a time-varying dynamics model in parallel channels;

[0294] A parameter optimization module is configured to construct a total cost function according to a total thrust size and the control torque, and iteratively optimize a candidate solution vector constructed by controller parameters of the trajectory tracking controller based on a particle swarm optimization algorithm to obtain real-time target controller parameters;

[0295] A trajectory tracking module is configured to dynamically adjust the trajectory tracking controller based on the real-time target controller parameters to continuously track a trajectory of the dynamic release of the sub-mother unmanned aerial vehicle.

[0296] The scheme verifies that the compound control strategy can still maintain the trajectory tracking accuracy and closed-loop stability of the centimeter-level aerial release of the sub-mother unmanned aerial vehicle under extreme working conditions through multi-scene and progressive simulation experiments, including high dynamic trajectories, 1.5 kg model mismatch, transient impact and continuous random noise, and fully proves the high-bandwidth disturbance suppression capability and engineering popularization value.

[0297] The above is only a specific embodiment of the present application, but the protection scope of the present application is not limited thereto, and any person skilled in the art can easily think of changes or replacements within the technical range disclosed by the present application, which should be covered within the protection scope of the present application.

Claims

1. A trajectory tracking control method for sub-munition UAV air release, characterized in that, Comprising the following steps: S1, based on the release state of the mother-son unmanned aerial vehicle, real-time calculation of time-varying parameters, and construction of time-varying dynamics model; The S1 comprises the following steps: S11, based on the release state of the mother-son unmanned aerial vehicle, setting the release state vector of the mother machine about the son machine; S12, based on the release state vector of the mother machine about the son machine, real-time calculation of time-varying parameters, wherein the time-varying parameters include total mass, total center of mass and total moment of inertia matrix; The calculation expression of time-varying parameters in the S12 is as follows: ， ， ， ， ， ， wherein, denotes the total mass of the master and all unreleased slaves at time t, denotes the mass of the master, denotes the total number of slaves, denotes the release status vector of the master with respect to the slave at time t, denotes the mass of the slave, denotes the mass of the slave, denotes the center of mass position of the master, denotes the center of mass position of the slave, denotes the total moment of inertia of the master and all unreleased slaves at time t, denotes the moment of inertia of the master about its center of mass, denotes the skew-symmetric matrix of the translation vector of the master, denotes the moment of inertia of the slave about its center of mass, denotes the skew-symmetric matrix of the translation vector of the slave, denotes the skew-symmetric matrix of the matrix, denotes the translation vector of the master, denotes the translation vector of the slave, where denotes the slave is not released at time t, where denotes the slave is released at time t. S13, according to the total center of mass in the time-varying parameter, based on the center of mass offset caused by the release of the mother machine and the rotor thrust action, the quasi-static disturbance torque and the transient impact disturbance are calculated, wherein the transient impact disturbance includes net recoil force and net recoil torque; The calculation expression of quasi-static disturbance torque and transient impact disturbance is as follows: ， ， ， wherein, represents the quasi-static disturbance moment, represents the total thrust generated by the rotor, represents the net recoil force on the total center of mass when a submachine is released, represents the release mechanism of the submachine on the mother machine, represents the counter-impact force received by the mother machine when releasing the submachine through the first release mechanism, represents the net recoil moment, represents the center of mass position of the first release mechanism; S14, according to the disturbed flow field at the moment of release of the son machine, the aerodynamic disturbance force and the aerodynamic disturbance torque are defined; S15, based on the parent machine coordinate system under the net recoil force, aerodynamic interference force and external disturbance force, construct the composite disturbance force; The calculation expression of the composite disturbance force is as follows: ， wherein, represents a complex disturbance force, represents a rotation matrix from the parent aircraft body coordinate system to the ground inertial coordinate system, represents an aerodynamic interference force, represents an external disturbance force; S16, time-varying inertia according to Newton Euler equation Derivation, get the moment of inertia mutation impact torque; S17, based on the parent machine coordinate system S17, based on the parent machine coordinate system S17, based on the parent machine coordinate system S17, based on the parent machine coordinate system S17, based on the parent machine coordinate system S17, based on the parent machine coordinate system S17, based on the parent machine coordinate system S17, based on the parent machine coordinate system S17, based on the parent machine coordinate system The calculation expression of the composite disturbance torque is as follows: ， wherein, represents a compound disturbance torque, represents an aerodynamic interference torque, represents a moment of inertia jump impact torque; S18, based on the composite disturbance force and the composite disturbance torque, the time-varying dynamics model is constructed; The calculation expression of the time-varying dynamics model is as follows: ， ， wherein, denotes the acceleration of the mother machine, denotes the total thrust generated by the rotors, denotes the gravitational acceleration vector, denotes the centripetal acceleration, denotes the angular acceleration of the mother machine, denotes the three-axis control moment in the mother machine body coordinate system denotes the three-axis control moment in the mother machine body coordinate system denotes the angular velocity of the mother machine, denotes the angular velocity matrix of the mother machine; S2, respectively constructing position three-order linear extended state observer, and three attitude two-order linear extended state observers in parallel channel; The S2 comprises the following steps: S21, based on the second-order integrator system reconstructed by the translational dynamics, a position three-order linear extended state observer is constructed, wherein the position three-order linear extended state observer is used to predict the lumped disturbance acceleration in real time; S22, based on the first-order integrator system reconstructed by the rotational dynamics, three attitude two-order linear extended state observers in parallel channel are constructed, wherein the three attitude two-order linear extended state observers in parallel channel are used to predict the lumped rotational disturbance in real time; The calculation expression of the attitude two-order linear extended state observer is as follows: ， wherein represents the predicted value of the angular acceleration of the represents the bandwidth of the attitude second order linear extended state observer, represents the predicted value of the angular velocity of the represents the rate of change of the predicted value of the lumped disturbance of the = 1, 2, 3;​​​​​​​ S3, based on the time-varying dynamics model and the position three-order linear extended state observer, the composite control law derivation and thrust solution are carried out, and the total thrust size and the expected attitude are obtained; The S3 comprises the following steps: S31, based on the time-varying dynamics model, the translational dynamics model is decoupled with acceleration as the core to obtain the decoupled translational dynamics model of the position controller; The calculation expression of the decoupled translational dynamics model of the position controller is as follows: ， ， ， ， wherein, represents the thrust acceleration, represents the known acceleration, represents the lumped disturbance acceleration, represents is defined as; S32, based on the position outer loop, the expected speed reference command is calculated according to the position error and the integral error; The calculation expression of the expected speed reference command is as follows: ， wherein, represents a position error, represents a position of the mother machine in a ground inertial coordinate system represents a position error, represents a trajectory position command, represents an integral error, represents a position error at a time instant, represents a time derivative, represents a desired velocity reference command, represents a tuning factor for a positive definite diagonal gain of position, represents a velocity of the mother machine in a ground inertial coordinate system represents a positive definite diagonal gain matrix of position, represents a positive definite diagonal gain matrix of position, represents a positive definite diagonal gain matrix of integral. S33, according to the expected speed reference command, the ideal feedback acceleration is calculated by using the speed loop according to the speed tracking error; The calculation expression of the ideal feedback acceleration is as follows: ， ， wherein, denotes the ideal feedback acceleration, denotes the velocity error dynamics, denotes the velocity positive definite diagonal gain matrix, denotes the velocity error; S34, according to the nonlinear dynamic inverse and the ideal feedback acceleration, the expected thrust acceleration is calculated based on the decoupled dynamics model of the position controller; The calculation expression of the expected thrust acceleration is as follows: ， wherein, represents the desired thrust acceleration, represents the lumped disturbance acceleration prediction; S35, according to the expected thrust acceleration and the real-time total mass in the time-varying parameter, the expected thrust vector is calculated; The calculation expression of the expected thrust vector is as follows: ， wherein, represents the desired thrust vector; S36, decompose the desired thrust vector to obtain the desired total thrust magnitude and the desired attitude; The calculation expression of the desired total thrust magnitude and the desired attitude is as follows: ， ， wherein, denotes the desired total thrust magnitude, denotes the desired total thrust magnitude, denotes the desired attitude; S4, calculate the command angular acceleration according to the desired attitude, define an error signal, and calculate the control torque based on the three attitude second-order linear extended state observers in parallel, the nonlinear dynamic inverse, and the time-varying dynamics model; The S4 includes the following steps: S41, derive the desired attitude with respect to time to calculate the command angular velocity, and filter the command angular velocity to calculate the command angular acceleration; S42, define an error signal, wherein the error signal includes an attitude error and an angular velocity tracking error; S43, construct a target angular acceleration based on the command angular acceleration and the error signal; The calculation expression of the target angular acceleration is as follows: ， wherein denotes a target angular acceleration, denotes a command angular acceleration, denotes an angular acceleration gain matrix, denotes an angular velocity tracking error, denotes a tuning factor for the attitude gain, denotes an attitude gain matrix, denotes an attitude error; S44, calculate the controller input angular acceleration of each channel based on the three attitude second-order linear extended state observers in parallel according to the target angular acceleration; The calculation expression of the controller input angular acceleration of each channel is as follows: ， wherein, represents the controller input angular acceleration of the represents the target desired angular acceleration of the channel.​ S45, calculate the control torque based on the nonlinear dynamic inverse and the rotational dynamics model according to the controller input angular acceleration of each channel, the target angular acceleration, and the time-varying dynamics model; The calculation expression of the control torque is as follows: ， wherein represents a predicted value of the collective disturbance, represents a known disturbance torque; S5, construct a total cost function based on the total thrust magnitude and the control torque, and iteratively optimize the candidate solution vector constructed by the controller parameters of the trajectory tracking controller based on the particle swarm optimization algorithm to obtain real-time target controller parameters; The S5 includes the following steps: S51, construct a total cost function based on the total thrust magnitude and the control torque; The calculation expression of the total cost function is as follows: ， ， ， ， ， wherein, represents the total cost of particles , represents the instantaneous performance cost, represents the instantaneous energy consumption cost, represents the mother machine release control end time, represents the mother machine release control start time, represents the time differential, represents the position-based weight, represents the axis, represents axis, represents axis, represents axis, represents the axis weight, represents the absolute value of the axis position error, represents the attitude weight, represents the attitude angle, represents the roll angle, represents the pitch angle, represents the yaw angle, represents the attitude weight of the attitude angle, represents the absolute value of the attitude error of the attitude angle, represents the global energy consumption weight, represents the thrust weight, represents the total thrust, represents the control moment de-weight of the axis, represents the control moment of the axis; S52, construct a candidate solution vector of the trajectory tracking controller based on the controller parameters of the trajectory tracking controller according to the physical symmetry of the mother-son unmanned aerial vehicle in the X-Y plane; S53, each candidate solution vector of the trajectory tracking controller is taken as a particle respectively ; S54, continuously iterate to obtain the optimal solution vector of the real-time trajectory tracking controller by taking the total cost function as the objective function according to the particle swarm optimization algorithm; The S54 includes the following steps: S541, initialize the particle swarm based on the parameter space of the candidate solution of the trajectory tracking controller and set the current iteration number to 0 according to the particle swarm algorithm; S542, execute the mother aircraft release control program based on the position and speed of each particle in the particle swarm, and extract the 3-axis position error, the 3-axis attitude error, the total thrust, and the 3-axis control torque as the simulation output of the particle; S543, evaluate the total cost of each particle in the particle swarm based on the simulation output of each particle and the total cost function; S544, update the individual historical optimal value and the global historical optimal value in the particle swarm according to the total cost of each particle; S545, update the speed and position of each particle according to the individual historical optimal value and the global historical optimal value in the particle swarm; S546, determine whether the current iteration number reaches the iteration number threshold, if yes, go to S547, otherwise, increase the current particle iteration number by 1 and return to S542; S547, the candidate solution vector corresponding to the global historical optimal value is taken as an optimal solution vector of the trajectory tracking controller; S55, the controller parameter corresponding to the optimal solution vector of the real-time trajectory tracking controller is taken as a real-time target controller parameter; S6, the trajectory tracking controller is dynamically adjusted based on the real-time target controller parameter, so as to continuously track the trajectory of the dynamic release of the sub-mother unmanned aerial vehicle.

2. A trajectory tracking control system based on the trajectory tracking control method for sub-mother unmanned aerial vehicle air release according to claim 1, characterized in that, Comprise: a parameter updating module, configured to calculate a time-varying parameter in real time based on the release state of the sub-mother unmanned aerial vehicle, and construct a time-varying dynamics model; a channel expansion observer module, configured to construct a position three-order linear expansion state observer, and three attitude two-order linear expansion state observers in parallel with the channel; a position outer loop module, configured to derive a compound control law and calculate a thrust based on the time-varying dynamics model and the position three-order linear expansion state observer, so as to obtain a total thrust size and an expected attitude; an attitude inner loop module, configured to calculate an instruction angular acceleration based on the expected attitude, define an error signal, and calculate a control torque based on the three attitude two-order linear expansion state observers in parallel with the channel, a nonlinear dynamic inverse, and the time-varying dynamics model; a parameter optimization module, configured to construct a total cost function based on the total thrust size and the control torque, and iteratively optimize a candidate solution vector constructed by controller parameters of the trajectory tracking controller based on a particle swarm optimization algorithm, so as to obtain a real-time target controller parameter; a trajectory tracking module, configured to dynamically adjust the trajectory tracking controller based on the real-time target controller parameter, so as to continuously track the trajectory of the dynamic release of the sub-mother unmanned aerial vehicle.

Images