Dynamic model adaptive trajectory tracking control method for deformable UAV

Abstract

The present invention discloses a dynamic model adaptive trajectory tracking control method for a deformable UAV, including: establishing a dynamic model of the deformable UAV; constructing a control framework based on the model, wherein: flat mapping is performed according to a given reference trajectory, the desired state feedforward term is calculated according to the current estimated or measured state, and flight control is realized according to the quadrotor geometric control theory; deformation control is realized based on feedback control of the servo motor angle error; the uncertainty factors in the grasping process of the deformable UAV are modeled as residual terms composed of external forces and corresponding torques, and the L1 adaptive control algorithm is used to adaptively compensate for the external forces, and the incremental nonlinear dynamic inverse control algorithm is used to compensate for the external torque, thereby realizing adaptive trajectory tracking control. Flight control is performed based on the dynamic model of the deformable UAV, and external forces and torques are observed and compensated in real time, effectively solving the uncertainty problem and the influence of external disturbances.

Description

Technical Field

[0001] The present invention belongs to the technical field of unmanned aerial vehicle flight control, and in particular relates to a dynamic model adaptive trajectory tracking control method for a deformable unmanned aerial vehicle. Background Art

[0002] The mechanical structure of a deformable UAV can change during flight, and the relative position of each rotor will also change dynamically. This dynamic change feature can make the entire aircraft more easily adapt to wide and narrow environments in terms of physical size. At the same time, the deformation mechanism can also replace the additional mounted robotic arm to a certain extent, which can meet the diverse applications of grabbing, transporting and perching specific objects in the air, greatly improving its work efficiency and practical benefits. However, the control strategy and algorithm of the deformable UAV platform are more complex than those of ordinary quadcopters. Not only must the aerodynamic changes of the quadcopters brought by the deformation mechanism and the changes in the body dynamics such as the overall rotational inertia of the aircraft be considered, but also the external disturbances such as load changes and airflow effects caused by aerial grabbing tasks must be solved. At present, conventional quadcopter flight control methods cannot achieve stable dynamic deformation flight of deformable UAVs.

[0003] Due to the non-steady-state characteristics of quadrotor drones, most commonly used flight attitude control algorithms use linear control methods such as PID and LQR to achieve stable attitude hovering and near-hovering flight. With the growing demand for large attitude and agile high-speed flight of drones, the nonlinear characteristics of the angular dynamics model of drones cannot be ignored. "Mellinger, D. and Kumar, V. (2011), Minimum snap trajectory generation and control for quadrotors, in 'IEEE International Conference on Robotics and Automation'" proposed a geometric trajectory tracking controller. This control method is based on the representation of manifolds, takes the differential flatness characteristics of quadrotor aircraft as the control feedforward term, and adopts a cascade control framework consisting of a position controller and an attitude controller. It is widely used in academia and industry.

[0004] For special quadrotor platforms designed for aerial grasping and deformation, controller design needs to consider the aerodynamic changes brought about by the additional deformation kinematic model and the model uncertainty and external disturbance problems brought about by aerial interaction scenarios such as grasping and operating objects. Currently, most deformable UAV controllers only consider the changes brought about by deformation. However, only considering the aerodynamic changes brought about by deformation cannot stably deal with the model changes and external disturbance problems brought about by aerial object grasping.

[0005] Under the demand for such strong air interaction, robust control or adaptive control strategies are generally used to ensure the safe operation of drones under model uncertainty. Some existing technologies are based on the assumption of static uncertainty and design adaptive control methods to solve the quadrotor control problem based on model uncertainty. Another part assumes a known prior uncertainty structure to design adaptive control laws. It is worth noting that these assumptions cannot accurately capture the time-varying and state-dependent model parameter uncertainties and cannot be universally applied to most scenarios. In addition, there are existing technologies that explore sliding film nonlinear control methods to solve the above problems. However, sliding mode control has a flutter problem, which will also cause instability of the entire control system. Summary of the invention

[0006] In view of the problems existing in the prior art, the purpose of the embodiments of the present application is to provide a dynamic model adaptive trajectory tracking control method for a deformable UAV.

[0007] According to a first aspect of an embodiment of the present application, a dynamic model adaptive trajectory tracking control method for a deformable UAV is provided, comprising:

[0008] Establish a dynamic model of a deformable UAV;

[0009] A control framework based on the dynamic model is constructed, in which: a flat mapping is performed according to a given reference trajectory, a desired state feedforward term is calculated according to the current estimated or measured state, and flight control is achieved according to the quadrotor geometric control theory; deformation control is achieved based on feedback control of the servo motor angle error;

[0010] The uncertainty factors in the grasping process of deformable UAV are modeled as residual terms composed of external force and corresponding torque. The L1 adaptive control algorithm is used to adaptively compensate the external force, and the incremental nonlinear dynamic inverse control algorithm is used to compensate the external torque, thereby realizing adaptive trajectory tracking control.

[0011] Furthermore, the dynamic model of the deformable UAV includes:

[0012] UAV translational dynamics equation:

[0013]

[0014] in is the total thrust applied to the Z axis of the body, is the representation of the Z axis of the body coordinate system in the world coordinate system. is the total mass, is the gravity vector, g is the acceleration due to gravity, Represents the force residual term caused by external disturbance force or model mismatch in the world coordinate system;

[0015] The UAV rotation dynamics equation:

[0016]

[0017] in represents the quaternion multiplication operator, is the total torque, is the deformation-dependent inertia matrix that changes with the deformation process, It is the torque residual term caused by external disturbance torque or model mismatch in the body coordinate system;

[0018] Total thrust and torque of the drone:

[0019]

[0020] in represents the Hadamard product, represents the torque caused by the propeller angular acceleration, represents the control allocation matrix related to the propeller position during the deformation of the UAV;

[0021] Deformation dynamics equation:

[0022]

[0023]

[0024]

[0025] The radius of the pulley is , the torque of the servo motor is The tension on the string is , the displacement of the servo motor is , the displacement of the thin rope is , for the A passive telescopic mechanism, the elastic coefficient of the compression spring is , the length change of the compression spring is , for the A passive rotating mechanism, the length of the arm of the string tension , the elastic constant of the torsion spring is , the angle change of the torsion spring is .

[0026] Furthermore, the control law of flight control is:

[0027]

[0028]

[0029] in is a positive definite gain matrix, They are respectively the expected acceleration, the estimated angular velocity, the expected angular velocity, the expected angular acceleration, the rotation matrix corresponding to the expected angle, and the transpose of the rotation matrix corresponding to the reference angle. , , , They are position error, velocity position, rotation error and angular velocity error corresponding to the angle, respectively. and are the desired position and the reference position respectively, and are the desired speed and the reference speed respectively, is the reference angular velocity;

[0030] The desired thrust of the UAV is calculated by the control law of the flight control and torque , and converted into the desired motor speed through the motor model to achieve flight control.

[0031] Furthermore, the desired speed of the servo motor is calculated based on the angle error, thereby realizing deformation control. Calculated by the following formula:

[0032]

[0033] The desired angle of the servo motor is , the current angle is , the reference speed is , To control the gain.

[0034] Furthermore, after compensation by the L1 adaptive control algorithm and the incremental nonlinear dynamic inverse control algorithm, the final flight control law of the UAV is as follows:

[0035]

[0036]

[0037] in is a positive definite gain matrix, They are respectively the expected acceleration, the estimated angular velocity, the expected angular velocity, the expected angular acceleration, the rotation matrix corresponding to the expected angle, and the transpose of the rotation matrix corresponding to the angle. , , , They are position error, velocity position, rotation error and angular velocity error corresponding to the angle, respectively. is the external force to be compensated obtained by the L1 adaptive control algorithm, is the external torque to be compensated obtained by the incremental nonlinear dynamic inverse control algorithm;

[0038] The desired thrust of the UAV is calculated through the final flight control law of the UAV and torque , and is converted into the desired motor speed through the motor model, thereby achieving adaptive trajectory tracking control.

[0039] According to a second aspect of an embodiment of the present application, a dynamic model adaptive trajectory tracking control device for a deformable UAV is provided, comprising:

[0040] Modeling module, used to build the dynamic model of the deformable UAV;

[0041] A control framework building module is used to build a control framework based on the dynamic model, in which: flat mapping is performed according to a given reference trajectory, a desired state feedforward term is calculated according to the current estimated or measured state, and flight control is realized according to the quadrotor geometric control theory; deformation control is realized based on feedback control of the servo motor angle error;

[0042] The adaptive compensation module is used to model the uncertainty factors in the grasping process of the deformable UAV as a residual term composed of external force and corresponding torque, use the L1 adaptive control algorithm to adaptively compensate the external force, and use the incremental nonlinear dynamic inverse control algorithm to compensate the external torque, thereby realizing adaptive trajectory tracking control.

[0043] According to a third aspect of an embodiment of the present application, a computer program product is provided, comprising a computer program / instruction, which implements the method described in the first aspect when executed by a processor.

[0044] According to a fourth aspect of an embodiment of the present application, there is provided an electronic device, including:

[0045] one or more processors;

[0046] A memory for storing one or more programs;

[0047] When the one or more programs are executed by the one or more processors, the one or more processors implement the method as described in the first aspect.

[0048] According to a fifth aspect of an embodiment of the present application, a computer-readable storage medium is provided, on which computer instructions are stored, and when the instructions are executed by a processor, the steps of the method described in the first aspect are implemented.

[0049] The technical solution provided by the embodiments of the present application may have the following beneficial effects:

[0050] It can be seen from the above embodiments that the present application establishes the state-space equation of the line-driven multi-rotor UAV to accurately describe the state of the UAV at different stages, and combines the model-based geometric control method to construct a trajectory tracking control architecture for the dynamic model; combines the nonlinear dynamic inversion and observer methods to accurately estimate the system state and disturbance, and combines the feedback linearization method to effectively compensate and feedback control the disturbance, resist the mass, aerodynamics and other disturbances of various loads, and the deformation flight control can meet the safety, stability and accurate control requirements in various typical application scenarios such as adaptive crossing of wide and narrow environments, aerial object grabbing and transportation, and aerial habitat, and solves the model uncertainty problems caused by flight grabbing and dynamic deformation in aerial interactive scenarios and the external disturbance problems of ground effects and wind to the greatest extent.

[0051] It should be understood that the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the present application. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] The accompanying drawings, which are incorporated in and constitute a part of this specification, illustrate embodiments consistent with the present application and, together with the description, serve to explain the principles of the present application.

[0053] Figure 1 is a schematic diagram of the control challenges of a deformable UAV in the background technology.

[0054] Figure 2 1 is a schematic diagram of the structure of a deformable UAV according to an exemplary embodiment, wherein (a) is a three-dimensional mechanical structure of conventional (maximum) size; (b) is a two-dimensional mechanical schematic diagram of conventional (maximum) size; (c) is a three-dimensional mechanical structure of compact (minimum) size after deformation; and (d) is a two-dimensional mechanical schematic diagram of compact (minimum) size after deformation.

[0055] Figure 3 The present invention is a flow chart of a dynamic model adaptive trajectory tracking control method for a deformable UAV according to an exemplary embodiment.

[0056] Figure 4 The figure is a schematic diagram of a coordinate system definition of a UAV according to an exemplary embodiment.

[0057] Figure 5 is a schematic diagram of a control framework according to an exemplary embodiment.

[0058] Figure 6 The figure-eight trajectory flight diagram of a UAV is shown according to an exemplary embodiment.

[0059] Figure 7 The figure is a graph showing the change of the hovering position of a deformable UAV in the air according to an exemplary embodiment.

[0060] Figure 8 This is a schematic diagram of a deformable UAV grabbing a load object in the air, where (a) is the position tracking curve, (b) is the speed tracking curve, (c) is the external force estimation curve, (d) is the external torque estimation curve, (e) is the servo motor angle change curve, and (f) is the servo motor torque change curve.

[0061] Fig. 9 Schematic diagram of a deformable UAV perching in the air, where (a) is a position tracking curve and (b) is a motor speed change curve.

[0062] Fig.10 The present invention is a block diagram of a dynamic model adaptive trajectory tracking control device for a deformable UAV according to an exemplary embodiment.

[0063] Fig.11 is a schematic diagram of an electronic device according to an exemplary embodiment.

[0064] Figure numerals: 1. first finger-imitation module; 2. second finger-imitation module; 3. first palm-imitation module; 4. second palm-imitation module; 5. servo motor; 6. fixed pulley; 7. compression spring; 8. thin rope; 9. slide rail; 10. slider; 11. propeller; 12. brushless motor; 13. circumferential bearing; 14. torsion spring. DETAILED DESCRIPTION

[0065] Here, exemplary embodiments will be described in detail, and examples thereof are shown in the accompanying drawings. When the following description refers to the drawings, unless otherwise indicated, the same numbers in different drawings represent the same or similar elements. The implementations described in the following exemplary embodiments do not represent all implementations consistent with the present application.

[0066] The terms used in this application are for the purpose of describing specific embodiments only and are not intended to limit this application. The singular forms of "a", "said" and "the" used in this application and the appended claims are also intended to include plural forms unless the context clearly indicates other meanings. It should also be understood that the term "and / or" used herein refers to and includes any or all possible combinations of one or more associated listed items.

[0067] It should be understood that although the terms first, second, third, etc. may be used in the present application to describe various information, these information should not be limited to these terms. These terms are only used to distinguish the same type of information from each other. For example, without departing from the scope of the present application, the first information may also be referred to as the second information, and similarly, the second information may also be referred to as the first information. Depending on the context, the word "if" as used herein may be interpreted as "at the time of" or "when" or "in response to determining".

[0068] The present application is implemented based on a deformable drone, which includes a first finger-imitation module 1, a first palm-imitation module 3, a second finger-imitation module 2 and a second palm-imitation module 4. A propeller 11 and a corresponding driver are arranged under each module, and a brushless motor 12 can be used as the driver of the corresponding propeller 11; the first finger-imitation module 1 and the first palm-imitation module 3, the first palm-imitation module 3 and the second palm-imitation module 4, and the second palm-imitation module 4 and the second finger-imitation module 2 are all rotationally connected through a composite under-actuated deformation structure; the composite under-actuated deformation structure is deformed by line drive, and includes a passive telescopic mechanism and a passive rotation mechanism, the passive telescopic mechanism is used to adjust the distance between the modules, and the passive rotation mechanism is used to rotate the finger-imitation module relative to the palm-imitation module to achieve bionic flexion and extension and grasping functions. Adjacent modules are connected by a passive telescopic mechanism, which includes a slider 10-slide rail 9 assembly and a compression spring 7. Preferably, two sets of sliders 10-slide rail 9 assemblies can be provided. Compared with a single set of sliders 10-slide rail 9 assemblies, this double-structure design can improve the stiffness of the connection. The compression spring 7 is mounted on one of the slide rails 9 to achieve recovery after expansion and contraction between modules. A passive rotation mechanism connection structure is adopted, which includes a torsion spring 14 and a circumferential bearing 13. The circumferential bearing 13 is used to reduce friction during rotation, and the torsion spring 14 is used to provide a driving force for recovery after rotation, so as to ensure that the finger-like module can rotate like a finger joint. The installation position of the passive rotation mechanism is as follows: Figure 2 As shown, the circumferential bearing 13 can reduce the friction when the fingertip-mimicking module rotates, and the torsion spring 14 can achieve deformation and shape recovery functions similar to the above-mentioned compression spring 7. The above-mentioned telescopic mechanism and rotation mechanism together constitute the composite bionic deformation structure of the drone, which has 5 degrees of freedom. The deformation structure is driven by only a single servo motor 5, so it has an under-driven characteristic.

[0069] In one embodiment, the finger-simulating module is provided with a double-layer structure, which is rotatably connected to the finger-simulating module by a bolt, and the sliders 10 of the two sets of sliders 10-slide rail 9 assemblies are respectively arranged on the upper plate and the lower plate of the double-layer structure, and one end of the slide rail 9 is correspondingly arranged on the top plate and the bottom plate of the palm-simulating module, and the circumferential bearing 13 and the torsion spring 14 are sleeved on the bolts and separated by the upper plate of the double-layer structure. In a specific implementation, in order to prevent the sliding from poking the internal components (such as the servo motor 5) of the finger-simulating module, the slider 10-slide rail 9 assembly arranged above between the finger-simulating module and the palm-simulating module can be set as follows: one end of the slide rail 9 is fixed to the upper plate of the double-layer structure, and the slider 10 is arranged at the other end of the slide rail 9 and is fixedly connected to the top plate of the palm-simulating module. In order to deform more flexibly, the remaining slider 10-slide rail 9 assembly can be provided with two sliders 10, and the compression spring 7 is arranged between the two sliders 10, and the two sliders 10 are respectively fixed on the modules to be connected.

[0070] In the specific implementation, a single motor can be used to realize the linear drive deformation, specifically: a number of light fixed pulleys 6 are installed on the inner side of each part of the fuselage, such as Figure 2 As shown, each palm-imitation module is provided with at least one fixed pulley 6, and the bottom plate and double-layer structure part of each finger-imitation module each include a fixed pulley 6. Next, one end of a nylon string 8 is fixed to the finger-imitation module, and the other end of the string 8 is wound in sequence along the groove direction of each fixed pulley 6, and finally connected to the turntable of the servo motor 5 of another finger-imitation module, forming the basic structure of the line drive. When the servo motor 5 of the drone starts to rotate, the string 8 is pulled to produce sliding, causing the pulley to produce a compression movement. At this time, the spring will be compressed, and the body will shrink to reduce its own size. When there is no grabbing load, since the tension on the string 8 is equal everywhere, the deformation of the three compression springs 7 is the same, so the displacement of the telescopic mechanism between the modules is consistent with each other; similarly, the rotational deformation of the two compression springs 7 is the same, and the rotation angles of the two finger-imitation modules are also the same. In this way, the body can achieve central symmetrical deformation under the drive of the servo motor 5, as shown in FIG. Figure 2As shown. In addition, in the face of the problem of the need for multiple power sources for multi-degree-of-freedom mechanisms, the drone is based on a driver method that produces the maximum utilization rate in an integrated configuration. It only uses one servo motor 5 to drive the thin rope 8 to retract in the deformation mechanism, pull the modules to produce deformation movement, and drive the body to produce under-driven deformation, so as to achieve the goal of taking into account the diverse grasping and high flexibility deformation of the bionic hand while significantly reducing the number of drivers and energy consumption. It should be noted that at this time, the spring coefficient of the compression spring 7 can be set to be smaller than that of the torsion spring 14. Under the same force drive of the servo motor 5, the deformation of the rotating mechanism is significantly smaller than that of the telescopic mechanism. Therefore, when the servo motor 5 pulls the thin rope 8 to drive the body to gradually deform, the imitation palm module will not significantly rotate and deform in the initial stage, so that the opening area that can be used for grasping is kept as large as possible between the two imitation finger modules to meet the grasping needs of more large-sized objects.

[0071] For the above-mentioned deformable UAV, this application proposes a dynamic model adaptive trajectory tracking control method for deformable UAV, which is mainly divided into three steps: establishing the kinematic and dynamic equations of the deformable UAV, designing the geometric control trajectory tracking framework of the deformable UAV, and model uncertainty and disturbance observation and compensation. The details are explained below.

[0072] Step S1: Establishing a dynamic model of a deformable UAV;

[0073] The aerodynamic model of the deformable drone belongs to the quadrotor model, and the kinematic and dynamic equations are established based on a six-degree-of-freedom rigid body. With the world coordinate system The definition of Figure 4 As shown. Given the position of the deformable drone in the world coordinate system , attitude quaternion representation , and line speed .at the same time, represents the angular velocity of the drone in the body coordinate system. The rotation speed of the four propellers is given by Given, is the speed of the i-th propeller. The center positions of these propellers are defined in the body coordinate system as , the center position of each propeller Specified by coordinates on the XY plane of the body coordinate system.

[0074] The translational dynamics of the drone are given by the following equation:

[0075]

[0076] in is the total thrust applied to the Z axis of the body, is the representation of the Z axis of the body coordinate system in the world coordinate system. is the total mass, is the gravity vector, g is the acceleration due to gravity, It represents the external disturbance force in the world coordinate system or the force residual term caused by the mismatch of the quadrotor drone model under the ideal state.

[0077] The UAV rotation dynamics are given by the following equation:

[0078]

[0079] in represents the quaternion multiplication operator, is the total torque, is the deformation-dependent inertia matrix that changes with the deformation process, It is the torque residual term caused by external disturbance torque or model mismatch in the body coordinate system.

[0080] The force generated by each propeller and torque The quadratic model approximation is given by

[0081]

[0082] in and represent thrust coefficient and torque coefficient respectively.

[0083] Observing the total thrust f, we can find that there is actually thrust only in the direction perpendicular to the body, so we use The total thrust T is expressed as the component along the Z axis of the machine system. Therefore, the total thrust and torque are:

[0084]

[0085] in, for The component of the machine system on the Z axis.

[0086] For the convenience of representation and calculation, the following equations are simplified:

[0087]

[0088] in represents the Hadamard product. Represents the torque caused by the propeller angular acceleration. Defined as:

[0089]

[0090] in It represents the moment of inertia of the propeller around the Z axis of the fuselage. represents the control allocation matrix related to the propeller position during the deformation of the drone and is defined as follows:

[0091]

[0092] In a specific implementation, the driver of the deformable drone is a single servo motor, which conducts force through a wire drive structure. When the turntable of the servo motor rotates, the traction light rope moves between the pulleys, driving the passive telescopic mechanism and the passive rotating mechanism to deform, thereby realizing the contraction and deformation of the body. Therefore, the present application constructs a deformation dynamics equation to control the motion of the deformation process.

[0093] Given the radius of the pulley , the torque of the servo motor , the tension on the line , and the servo motor displacement and the displacement of the light rope The kinetic equation modeling is shown below:

[0094]

[0095] Therefore, servo control of the line position actually depends on servo motor position control. The movement of the line in turn applies force to the compression spring and torsion spring and changes the position of the propeller. A passive telescopic mechanism (including a slider-rail assembly and a compression spring), assuming that the elastic coefficient of the compression spring is , then the length change of the compression spring is :

[0096]

[0097] in At the same time, it is the displacement of the passive telescopic mechanism being compressed, that is, the change in the distance between adjacent modules. A passive rotating mechanism (including torsion spring and circumferential bearing), assuming that the length of the lever arm of the string tension is , the elastic constant of the torsion spring is , then the angle change of the torsion spring is :

[0098]

[0099] in is the torque exerted by the rope on the rotating mechanism, At the same time, it represents the rotation angle change of the module where the rotating mechanism is located. Therefore, through the above modeling, the position change and angle change of each module during no-load deformation can be calculated, laying a foundation for deformation control.

[0100] Step S2: constructing a control framework based on the dynamic model, wherein: a flat mapping is performed according to a given reference trajectory, a desired state feedforward term is calculated according to the current estimated or measured state, and flight control is implemented according to the quadrotor geometric control theory; deformation control is implemented based on feedback control of the servo motor angle error;

[0101] In terms of flight control, according to the dynamic model in step S1, a trajectory tracking control framework based on this dynamic change model is designed. This architecture performs a flat mapping of the quadrotor drone according to a given reference trajectory (including reference position and reference yaw angle) [Mellinger, D. and Kumar, V. (2011), Minimum snap trajectory generation and control for quadrotors, in 'IEEE International Conference on Robotics and Automation'], and calculates the desired state feedforward term (including reference position, reference velocity, reference angle and reference angular velocity, etc.) according to the current estimated or measured state. Then, based on the quadrotor geometric control theory, a trajectory tracking controller for the quadrotor drone is constructed. The entire controller is Figure 5 Operates within the cascade control framework shown.

[0102] Specifically, the controller covers the position, velocity and linear acceleration tracking controllers in the outer loop control part, and the inner loop control part ensures the accurate tracking of attitude, angular rate and angular acceleration. The detailed control law is as follows:

[0103]

[0104]

[0105] in is the positive definite gain matrix selected by the user, They are the expected acceleration, estimated angular velocity, expected angular velocity, expected angular acceleration, the rotation matrix corresponding to the expected angle, and the transpose of the rotation matrix corresponding to the reference angle. , , , They are position error, velocity position, rotation error and angular velocity error corresponding to the angle, respectively. and are the desired position and the reference position respectively, and are the desired speed and the reference speed respectively, is the reference angular velocity. Through the above control law, the desired thrust of the UAV can be calculated and torque Finally, the quad-rotor motor model is used to convert the torque and thrust of the drone into motor speed to achieve drone flight control.

[0106] During the deformation of the drone, the moment of inertia of the body and control allocation matrix is dependent on the servo motor angle used for deformation The status is updated in real time, which can ensure the stability and accuracy of trajectory tracking control under dynamic model updating during the deformation process.

[0107] In terms of deformation control, the servo motor needs to be motion controlled to achieve deformation control of the drone. To this end, a feedback controller based on angle error is designed. Assuming that the desired angle of the servo motor is , the current angle is , the reference speed is , is the control gain, then the desired speed of the servo motor is:

[0108]

[0109] The above flight controller and deformation controller are quasi-decoupled. The trajectory tracking architecture based on the geometric tracking controller can provide a basic flight framework for the UAV and realize precise control of the UAV under normal flight motion; the deformation controller based on angle error provides guarantee for the deformation of the body.

[0110] Step S3: using the L1 adaptive control algorithm to adaptively compensate for the external force, and using the incremental nonlinear dynamic inverse control algorithm to compensate for the external torque, thereby realizing adaptive trajectory tracking control;

[0111] It is worth noting that variable structure UAVs may bring additional disturbances and model uncertainties when deforming and grasping objects, which is not conducive to flight control. Therefore, this application proposes the observation and compensation of model uncertainty and disturbance. The specific contents are as follows. In step S2, the trajectory tracking framework based on the dynamic model can perform trajectory tracking when the UAV flight dynamics model is relatively accurate. This section will focus on the effects of model uncertainty such as variable loads during aerial grasping and airflow disturbances. The model uncertainty and disturbance problems are modeled equally as residual terms composed of external forces and torques, and algorithms are designed to estimate and compensate for external forces and torques respectively. An observation compensation module is added to the dynamic control part, such as Figure 6 shown.

[0112] This method combines the L1 adaptive control algorithm and the incremental nonlinear dynamic inverse control algorithm to estimate and compensate for external forces and torques respectively. The L1 adaptive algorithm based on the observer method refers to "Wu Z, Cheng S, Ackerman KA, et al. L 1 adaptive augmentation for geometric tracking control of quadrotors[C]. 2022 International Conference on Robotics and Automation(ICRA). IEEE, 2022: 1329-1336", which observes the speed and other states to estimate the external force to be compensated. ;

[0113] The incremental nonlinear dynamic inversion (INDI) algorithm relies on the rotor motor tachometer and inertial sensor (IMU) data to directly calculate the residual term through the measurement value. The main reference is "Tal E, Karaman S. Accurate tracking of aggressive quadrotortrajectories using incremental nonlinear dynamic inversion and differentialflatness[J]. IEEE Transactions on Control Systems Technology, 2020, 29(3):1203-1218." When actually deployed on a variable structure UAV, it is necessary to update the physical parameters of the UAV, such as the inertia tensor Finally, the external disturbance torque to be compensated is calculated as .

[0114] According to the dynamic characteristics of the deformable UAV, the L1 adaptive controller is applied to the adaptive compensation of external force, and the incremental nonlinear dynamic inverse control algorithm is applied to the compensation of external torque, which can effectively deal with the disturbance caused by operations such as deformation and grasping. After adding L1 adaptive external force compensation and incremental nonlinear dynamic inverse control torque compensation, the final flight control law of the UAV is as follows

[0115]

[0116]

[0117] In order to verify the effectiveness of the control method proposed in this application, a series of simulation and physical experiments were carried out.

[0118] In the simulation experiment, relevant variables are fixed for verification. The experiment mainly covers undeformed hovering flight, deformed hovering flight, undeformed figure-8 maneuvering flight, and deformed figure-8 maneuvering flight experiments before and after carrying a load, to ensure that the effects of deformation, aerial grabbing and other factors on the controller can be observed.

[0119] Specific experimental conditions:

[0120] 1) Hovering flight: The drone performs hovering flight at a height of 1 meter.

[0121] 2) Figure-8 maneuver flight: The drone flies along a given figure-8 trajectory with a width of 4.5 meters and a length of 10 meters at a maximum speed of 5 meters per second. Figure 6 shown.

[0122] 3) Deformed and undeformed states: The undeformed state is the original maximum volume state, and the deformed state is the original maximum volume and the contracted minimum volume state, switching between the deformations at the fastest speed.

[0123] 4) Carrying load state: Grab a 200g load at the front of the fuselage.

[0124] According to the above experimental settings, the effectiveness of the adaptive controller design proposed in this patent for variable structure UAVs in multiple scenarios is verified. The parameters of the corresponding geometric controller, INDI controller and L1 adaptive controller are shown in Table 1.

[0125] surface Parameters of geometric controller, INDI controller and L1 adaptive controller

[0126]

[0127] in, is the cut-off frequency of the motor speed filter, is the Hurwitz matrix of the UAV thrust (making the thrust prediction error gradually approach 0), is the Hurwitz matrix of the UAV torque (making the torque prediction gradually approach 0 with error), is the cutoff frequency of the thrust estimation filter, is the cutoff frequency of the moment estimation filter.

[0128] In the simulation environment, the deformable UAV is used to compare the traditional geometric controller, the geometric controller and the L1 adaptive force compensation strategy, the geometric controller and the incremental dynamic inverse torque compensation strategy, and the geometric controller and the L1 adaptive force compensation fusion incremental dynamic inverse torque compensation strategy under different experimental conditions. The final flight tracking error performance is shown in Table 2.

[0129] surface Comparison of control tracking errors of different controllers under different experimental conditions (mean error ± standard deviation, unit: meter)

[0130]

[0131] It can be clearly seen that the incremental dynamic inverse torque compensation strategy fused with the geometric controller and L1 adaptive force compensation shows better tracking accuracy than other single strategies under various experimental conditions. It can not only effectively adapt to the disturbance caused by deformation, but also accurately compensate for the impact of load on flight. At the same time, it can well cope with the problem of high-speed trajectory tracking in maneuverable flight, thus significantly improving the control accuracy and robustness of the system. This shows the effectiveness of the adaptive control strategy in dealing with complex flight conditions such as deformable and loaded flight.

[0132] In the real experiment, the method was deployed on a real deformable drone to verify various applications, including hovering deformation in the air, grabbing load objects in the air, and perching in the air. The reliability and accuracy of the control system proposed by the invention are guaranteed in the case of multiple motion mode switching and different structured scenes.

[0133] 1) Hovering in the air and deformation

[0134] Figure 7 The deformation process of the deformable UAV is shown. It can be seen from the deformation action sequence that the overall posture of the UAV is relatively stable during deformation, and the UAV changes symmetrically during the deformation process, showing the good effect of deformation control. At the same time, the position information of the UAV is shown in the figure, where the dotted box represents the position change of the UAV during the deformation process. It can be seen that the positions of the three axes X, Y, and Z are basically stable during deformation, and there is no obvious drift. The proposed controller can provide stable position control for the UAV during deformation.

[0135] 2) Grab the load object

[0136] The deformable drone can grab objects such as a water bottle filled with water by deforming itself. Humans usually grab larger objects (such as water bottles and oranges) with their palms, and the deformable drone also shows similar capabilities. In one embodiment, an autonomous grasping experiment was conducted, requiring the drone to grab a water bottle filled with water (diameter: 62 mm, mass: 153 g) and then transport it to the destination. Figure 8 The position and velocity tracking curves shown in the figure show that the drone can track the reference trajectory more accurately at a speed greater than 1.0m / s. The figure shows the real-time estimation of the external disturbance force and torque disturbance of the mineral water bottle, and finally effectively compensates for them to achieve more accurate position tracking of the drone.

[0137] 3) Aerial habitat

[0138] The deformable drone can use its special structure to grab onto a fixed object to achieve aerial perching. In order to evaluate the perching ability, another embodiment sets a tree trunk as a test object. Fig. 9 As shown in the figure, in this experiment, the deformable drone accurately grasped and perched on the tree. The motor speed curve in the figure shows that when perched, all motors stop rotating, the gravity of the system is offset by the friction generated by the drone grasping the tree trunk, and the energy consumption of the system is significantly reduced (the energy consumed by the servo motor is two orders of magnitude less than that of the rotor propeller). After a short stay, the drone received a command to take off again, and the propeller began to rotate again. The servo motor-driven drone then expanded, gradually detached from the tree, and re-entered the flight mode. This unique stay-retake mechanism allows rest between consecutive missions and supports long-term stay missions with low energy consumption.

[0139] In summary, this method develops a nonlinear adaptive trajectory tracking control method based on a dynamic model for the above-mentioned deformable UAV, covering the following contents:

[0140] 1. Accurately model the dynamic deformation of deformable UAVs and adapt to the changes in the dynamic model parameters of the UAV dynamic deformation;

[0141] 2. Build a trajectory tracking control framework based on dynamic models to achieve safe, reliable and accurate control of aerial gripping integrated drones in different aerial operation application scenarios;

[0142] 3. Observation methods and adaptive control law algorithms for external disturbances to achieve real-time observation and compensation of model uncertainties and external disturbances.

[0143] Deformable UAVs have the characteristic that the dynamic characteristics of the end effector change with different motion modes, and the aerial operation scenario has high requirements for the stability and robustness of the control; UAVs have disturbance problems such as ground effect and variable load in aerial operation tasks, and compensating for external disturbances and the uncertainty of the system dynamics model are also core issues that affect the control accuracy. Therefore, studying the adaptive control technology of deformable UAVs is a key issue to improve the control performance and stability of variable structure UAVs under multiple motion modes. This method is based on the cascade control framework of geometric control, decouples translation and rotation dynamics, models the deformation dynamics of UAVs, adopts the sensor measurement method of incremental nonlinear dynamic inversion and the disturbance observation strategy of L1 adaptive control for aerial operations and disturbances, and estimates the disturbances of external forces and torques in real time, ensuring the control stability of UAVs based on flight operation integration in typical application scenarios such as aerial grasping, deformation crossing, and perching.

[0144] Corresponding to the aforementioned embodiment of the dynamic model adaptive trajectory tracking control method for a deformable UAV, the present application also provides an embodiment of a dynamic model adaptive trajectory tracking control device for a deformable UAV.

[0145] Fig.10 1 is a block diagram of a dynamic model adaptive trajectory tracking control device for a deformable UAV according to an exemplary embodiment. Fig.10 , the device may include:

[0146] A modeling module 21, used to establish a dynamic model of the deformable UAV;

[0147] A control framework building module 22 is used to build a control framework based on the dynamic model, in which: a flat mapping is performed according to a given reference trajectory, a desired state feedforward term is calculated according to the current estimated or measured state, and flight control is realized according to the quadrotor geometric control theory; deformation control is realized based on feedback control of the servo motor angle error;

[0148] The adaptive compensation module 23 is used to model the uncertainty factors in the grasping process of the deformable UAV as a residual term composed of external force and corresponding torque, use the L1 adaptive control algorithm to adaptively compensate for the external force, and use the incremental nonlinear dynamic inverse control algorithm to compensate for the external torque, thereby realizing adaptive trajectory tracking control.

[0149] Regarding the device in the above embodiment, the specific manner in which each module performs operations has been described in detail in the embodiment of the method, and will not be elaborated here.

[0150] For the device embodiment, since it basically corresponds to the method embodiment, the relevant parts can refer to the partial description of the method embodiment. The device embodiment described above is only schematic, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the present application scheme. A person of ordinary skill in the art can understand and implement it without paying any creative work.

[0151] Accordingly, the present application also provides a computer program product, including a computer program / instruction, which, when executed by a processor, implements the dynamic model adaptive trajectory tracking control method for a deformable UAV as described above.

[0152] Accordingly, the present application also provides an electronic device, comprising: one or more processors; a memory for storing one or more programs; when the one or more programs are executed by the one or more processors, the one or more processors implement the above-mentioned dynamic model adaptive trajectory tracking control method for deformable drones. Fig.11 As shown in FIG. 1 , a hardware structure diagram of a dynamic model adaptive trajectory tracking control device for a deformable UAV provided by an embodiment of the present invention is provided for any device with data processing capability, except Fig.11 In addition to the processor, memory and network interface shown, any device with data processing capability in which the apparatus in the embodiment is located may also include other hardware, generally based on the actual functions of the device with data processing capability, which will not be described in detail.

[0153] Accordingly, the present application also provides a computer-readable storage medium on which computer instructions are stored, and when the instructions are executed by the processor, the dynamic model adaptive trajectory tracking control method for a deformable drone as described above is implemented. The computer-readable storage medium can be an internal storage unit of any device with data processing capabilities described in any of the aforementioned embodiments, such as a hard disk or a memory. The computer-readable storage medium can also be an external storage device, such as a plug-in hard disk, a smart memory card (Smart Media Card, SMC), an SD card, a flash card (FlashCard), etc. equipped on the device. Furthermore, the computer-readable storage medium can also include both an internal storage unit and an external storage device of any device with data processing capabilities. The computer-readable storage medium is used to store the computer program and other programs and data required by any device with data processing capabilities, and can also be used to temporarily store data that has been output or is to be output.

[0154] Those skilled in the art will readily appreciate other embodiments of the present application after considering the specification and practicing the contents disclosed herein. The present application is intended to cover any variations, uses or adaptations of the present application, which follow the general principles of the present application and include common knowledge or customary technical means in the art that are not disclosed in the present application.

[0155] It will be appreciated that the present application is not limited to the exact construction that has been described above and shown in the drawings, and that various modifications and changes may be made without departing from the scope thereof.

Claims

1. A dynamic model adaptive trajectory tracking control method for a deformable UAV, characterized in that: include: Establish a dynamic model of a deformable UAV; A control framework based on the dynamic model is constructed, in which: a flat mapping is performed according to a given reference trajectory, a desired state feedforward term is calculated according to the current estimated or measured state, and flight control is achieved according to the quadrotor geometric control theory; deformation control is achieved based on feedback control of the servo motor angle error; The uncertainty factors in the grasping process of the deformable UAV are modeled as residual terms composed of external forces and corresponding torques. The L1 adaptive control algorithm is used to adaptively compensate for the external forces, and the incremental nonlinear dynamic inverse control algorithm is used to compensate for the external torque, thereby realizing adaptive trajectory tracking control. The dynamic model of the deformable UAV includes: UAV translational dynamics equation: ， in is the position of the deformable drone in the world coordinate system, is the linear speed of the drone, is the total thrust applied to the Z axis of the body, is the representation of the Z axis of the body coordinate system in the world coordinate system. is the total mass, is the gravity vector, g is the acceleration due to gravity, Represents the force residual term caused by external disturbance force or model mismatch in the world coordinate system; The UAV rotation dynamics equation: ， in is the attitude quaternion of the drone, is the angular velocity of the UAV in the body coordinate system, represents the quaternion multiplication operator, is the total torque, is the deformation-dependent inertia matrix that changes with the deformation process, It is the torque residual term caused by external disturbance torque or model mismatch in the body coordinate system; Total thrust and torque of the drone: ， in represents the Hadamard product, represents the torque caused by the propeller angular acceleration, represents the control allocation matrix related to the propeller position during the deformation of the drone. During the deformation of the drone, and Depends on the state of the servo motor angle used for deformation for real-time update; Deformation dynamics equation: ， ， ， ， ， The radius of the pulley is , the torque of the servo motor is The tension on the string is , the displacement of the servo motor is , the displacement of the thin rope is , is the torque exerted by the rope on the rotating mechanism. A passive telescopic mechanism, the elastic coefficient of the compression spring is , the length change of the compression spring is , for the A passive rotating mechanism, the length of the arm of the string tension , the elastic constant of the torsion spring is , the angle change of the torsion spring is ; The control law for flight control is: ， ， in is a positive definite gain matrix, They are respectively the expected acceleration, the estimated angular velocity, the expected angular velocity, the expected angular acceleration, the rotation matrix corresponding to the expected angle, and the transpose of the rotation matrix corresponding to the reference angle. , , , They are position error, velocity position, rotation error and angular velocity error corresponding to the angle, respectively. and are the desired position and the reference position respectively, and are the desired speed and the reference speed respectively, is the reference angular velocity; The desired thrust of the UAV is calculated by the control law of the flight control and torque , and converted into the desired motor speed through the motor model to achieve flight control; The expected speed of the servo motor is calculated based on the angle error, so as to realize deformation control. The expected speed of the servo motor Calculated by the following formula: ， The desired angle of the servo motor is , the current angle is , the reference speed is , To control the gain.

2. The method according to claim 1, characterized in that After compensation by the L1 adaptive control algorithm and the incremental nonlinear dynamic inverse control algorithm, the final flight control law of the UAV is as follows: ， ， in is a positive definite gain matrix, They are respectively the expected acceleration, the estimated angular velocity, the expected angular velocity, the expected angular acceleration, the rotation matrix corresponding to the expected angle, and the transpose of the rotation matrix corresponding to the angle. , , , They are position error, velocity position, rotation error and angular velocity error corresponding to the angle, respectively. is the external force to be compensated obtained by the L1 adaptive control algorithm, is the external torque to be compensated obtained by the incremental nonlinear dynamic inverse control algorithm; The desired thrust of the UAV is calculated through the final flight control law of the UAV and torque , and is converted into the desired motor speed through the motor model, thereby achieving adaptive trajectory tracking control.

3. A device for the dynamic model adaptive trajectory tracking control method for a deformable UAV as claimed in claim 1, characterized in that: include: Modeling module, used to build the dynamic model of the deformable UAV; A control framework building module is used to build a control framework based on the dynamic model, in which: flat mapping is performed according to a given reference trajectory, a desired state feedforward term is calculated according to the current estimated or measured state, and flight control is realized according to the quadrotor geometric control theory; deformation control is realized based on feedback control of the servo motor angle error; The adaptive compensation module is used to model the uncertainty factors in the grasping process of the deformable UAV as a residual term composed of external force and corresponding torque, use the L1 adaptive control algorithm to adaptively compensate the external force, and use the incremental nonlinear dynamic inverse control algorithm to compensate the external torque, thereby realizing adaptive trajectory tracking control.

4. A computer program product comprising a computer program / instructions, characterized in that When the computer program / instructions are executed by a processor, the method according to any one of claims 1 to 2 is implemented.

5. An electronic device, characterized in that: include: one or more processors; A memory for storing one or more programs; When the one or more programs are executed by the one or more processors, the one or more processors implement the method according to any one of claims 1 to 2.

6. A computer-readable storage medium having computer instructions stored thereon, characterized in that: When the instruction is executed by a processor, the steps of the method according to any one of claims 1 to 2 are implemented.

Images