Unmanned aerial vehicle formation control method and device, unmanned aerial vehicle and storage medium

Abstract

The invention discloses an unmanned aerial vehicle formation control method and device, unmanned aerial vehicles and a storage medium, and the method comprises the steps: taking each unmanned aerial vehicle in an unmanned aerial vehicle formation as a node, building a virtual rigid structure, calculating a virtual mass point of the rigid structure, and building a reference coordinate system employing the virtual mass point as an original point; projecting each unmanned aerial vehicle to the reference coordinate system, and calculating the relative position between each unmanned aerial vehicle and the virtual mass point; determining an actual formation configuration based on the current state of the virtual mass point, the current state of each unmanned aerial vehicle and the relative position; and calculating a difference value between the expected formation configuration and the actual formation configuration, determining a relative position of each unmanned aerial vehicle and the virtual mass point at the next moment based on the difference value, and controlling each unmanned aerial vehicle based on the relative position at the next moment. According to the scheme, formation control is realized through the expected trajectory and the expected configuration of the virtual mass point, and the method has relatively good rapidity and robustness.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of unmanned aerial vehicle formation control, and in particular to an unmanned aerial vehicle formation control method and device, an unmanned aerial vehicle and a storage medium. BACKGROUND

[0002] With the rapid development of unmanned aerial vehicle technology, the research on unmanned aerial vehicle formation technology has gradually attracted widespread attention. At present, researchers are exploring various unmanned aerial vehicle formation forms and application scenarios, covering military, civilian, commercial and other fields, which has important practical significance and broad application prospects, and will bring great development opportunities and economic benefits to the military, civilian and commercial fields.

[0003] However, many current researches on unmanned aerial vehicle formation are still at the theoretical stage, and generally a lead unmanned aerial vehicle is set when forming a formation, and the positions of other unmanned aerial vehicles are determined according to the lead unmanned aerial vehicle to maintain the formation. If the lead unmanned aerial vehicle fails or its information cannot be obtained, the unmanned aerial vehicle formation cannot operate normally. SUMMARY

[0004] In view of the above problems, the present application is proposed in order to provide an unmanned aerial vehicle formation control method, device, unmanned aerial vehicle and storage medium which overcome the above problems or at least partially solve the above problems.

[0005] According to one aspect of the present application, there is provided an unmanned aerial vehicle formation control method, which comprises:

[0006] Each unmanned aerial vehicle in the unmanned aerial vehicle formation is taken as a node to establish a virtual rigid structure, a virtual mass point of the rigid structure is calculated, and a reference coordinate system with the virtual mass point as the origin is established;

[0007] Each unmanned aerial vehicle is projected onto the reference coordinate system, and the relative position between each unmanned aerial vehicle and the virtual mass point is calculated;

[0008] Based on the current state of the virtual mass point, the current state of each unmanned aerial vehicle and the relative position, an actual formation configuration is determined;

[0009] The difference between the expected formation configuration and the actual formation configuration is calculated, and the relative position of each unmanned aerial vehicle and the virtual mass point at the next moment is determined based on the difference, and each unmanned aerial vehicle is controlled based on the relative position at the next moment.

[0010] In some embodiments, projecting each unmanned aerial vehicle onto the reference coordinate system and calculating the relative position between each unmanned aerial vehicle and the virtual mass point specifically comprises:

[0011] construct a first coordinate system, determine kinematic models of the virtual mass point and each of the UAVs in a height channel and a horizontal channel, and the kinematic model of the horizontal channel of the UAV includes external disturbances;

[0012] determine a relative distance between the UAVs and the virtual mass point in the first coordinate system and an angle between the relative distance and an X-axis of the first coordinate system, and obtain a relative position formula of each of the UAVs relative to the virtual mass point based on the relative distance, the angle, and the kinematic models.

[0013] In some embodiments, a difference between a target formation configuration and the initial formation configuration is calculated, and a relative position of each of the UAVs relative to the virtual mass point at a next time is determined based on the difference, and the control of each of the UAVs based on the relative position at the next time includes:

[0014] determining an acting force of each of the UAVs in a vertical direction based on a height difference in the relative position at the next time;

[0015] determining an attitude control parameter of each of the UAVs based on a difference in a horizontal direction and a yaw angle difference in the relative position at the next time.

[0016] In some embodiments, a difference between a target formation configuration and the initial formation configuration is calculated, and a relative position of each of the UAVs relative to the virtual mass point at a next time is determined based on the difference, and the control of each of the UAVs based on the relative position at the next time further includes:

[0017] determining an acting force parameter and / or an attitude control parameter of each of the UAVs based on the relative position at the next time by using a pre-constructed model compensation controller;

[0018] The model compensation controller is a controller based on a self-disturbance control principle, including a high-order differentiator, a compensation function observer, and a model compensation control law.

[0019] In some embodiments, the method further includes:

[0020] determining a dynamic equation of the UAV based on a reference coordinate system and a body coordinate system;

[0021] determining an interference force formula and an interference torque formula of the UAV based on external interference force factors to which the UAV is subjected;

[0022] determining a relationship formula between a control torque and an acting force of the UAV and a propeller rotation speed based on the dynamic equation, the interference force formula, and the interference torque;

[0023] The position system formula and attitude system formula of the UAV are determined based on the relationship formula.

[0024] In some embodiments, the method further comprises:

[0025] Simplifying the position system formula and the attitude system formula to obtain simplified formulas;

[0026] The force parameter and / or posture control parameter are determined based on the simplified formula and the model compensation controller.

[0027] In some embodiments, calculating a difference between the target formation configuration and the initial formation configuration, determining a relative position of each of the drones and the virtual particle at a next moment based on the difference, and controlling each of the drones based on the relative position at the next moment further includes:

[0028] The acting force and the attitude control parameters are converted and distributed to obtain the desired attitude signal required by the inner loop of each UAV, and then determine the motor speed control value of each UAV rotor.

[0029] According to another aspect of the present invention, a drone formation control device is provided, the device comprising:

[0030] a mass point calculation module adapted to establish a virtual rigid structure using each drone in the drone formation as a node, calculate a virtual mass point of the rigid structure, and establish a reference coordinate system with the virtual mass point as the origin;

[0031] a position determination module, adapted to project each of the drones onto the reference coordinate system and calculate the relative position between each of the drones and the virtual mass point;

[0032] a formation determination module, adapted to determine an actual formation configuration based on the current state of the virtual particle, the current state of each of the UAVs, and the relative position;

[0033] The position control module is adapted to calculate the difference between the expected formation configuration and the actual formation configuration, determine the relative position of each of the UAVs and the virtual particle at a next moment based on the difference, and control each of the UAVs based on the relative position at the next moment.

[0034] According to another aspect of the present invention, a drone is provided, comprising: a processor and a memory arranged to store computer-executable instructions, wherein when the executable instructions are executed, the processor executes the drone formation control method according to any one of the above embodiments.

[0035] According to still another aspect of the present application, there is provided a computer readable storage medium, wherein the computer readable storage medium stores one or more programs which, when executed by a processor, implement the unmanned aerial vehicle formation control method according to any one of the above.

[0036] As can be seen from the above, according to the unmanned aerial vehicle formation control method disclosed in the present application, the virtual structure method is adopted to control the formation, and the core idea is to make each unmanned aerial vehicle follow a mass point on a moving rigid structure. This scheme can well avoid the problem that the entire formation is paralyzed when the leader of other formation control methods is damaged or cannot work normally. After the formation shape and the virtual mass point are determined, other unmanned aerial vehicles will move following the moving virtual mass point, and each unmanned aerial vehicle in the formation can track the ideal trajectory of the virtual mass point to maintain the structure of the formation.

[0037] Further, in the process of maintaining the formation by the unmanned aerial vehicles, in addition to tracking the trajectory of the given virtual mass point, each unmanned aerial vehicle also needs to resist the disturbance from other unmanned aerial vehicles in the formation. This has certain requirements for the rapidity and robustness of the unmanned aerial vehicle control. In the present embodiment, the model compensation control (MCC) is adopted as the controller of the unmanned aerial vehicle to improve the rapidity and anti-disturbance of the trajectory tracking of the system.

[0038] The above description is only a summary of the technical solutions of the present application. In order to enable the technical means of the present application to be more clearly understood, and to be implemented according to the content of the specification, and in order to enable the above and other purposes, characteristics and advantages of the present application to be more apparent and easy to understand, the following specific embodiments of the present application are described. BRIEF DESCRIPTION OF DRAWINGS

[0039] Various other advantages and benefits will become apparent to those of ordinary skill in the art upon reading the following detailed description of the preferred embodiments. The accompanying drawings are included to provide a description of the preferred embodiments and are not meant to limit the present application. Furthermore, the same reference numerals are used throughout the several drawings to designate the same or similar parts. In the drawings:

[0040] Figure 1 A flowchart of an unmanned aerial vehicle formation control method according to an embodiment of the present application is shown;

[0041] Figure 2 A schematic diagram of an unmanned aerial vehicle formation control framework according to an embodiment of the present application is shown;

[0042] Figure 3 A schematic diagram of the relationship between the virtual center of mass and other unmanned aerial vehicles in a virtual structure according to an embodiment of the present application is shown;

[0043] Figure 4 A schematic diagram of the structure of a four-rotor unmanned aerial vehicle under stress according to an embodiment of the present application is shown.

[0044] Figure 5 Fig. 1 shows a structural schematic diagram of a model compensation controller according to an embodiment of the present application;

[0045] Figure 6 Fig. 2 shows a structural schematic diagram of a UAV formation control device according to an embodiment of the present application;

[0046] Figure 7 Fig. 3 shows a structural schematic diagram of a UAV (flight control system) according to an embodiment of the present application. DETAILED DESCRIPTION

[0047] Exemplary embodiments of the present application will be described herein below with reference to the accompanying drawings. Although exemplary embodiments of the present application are shown in the drawings, it should be understood that the present application can be implemented in various forms and should not be limited by the embodiments set forth herein. Rather, these embodiments are provided so that this application will be thorough and complete, and will fully convey the scope of the application to those skilled in the art.

[0048] Figure 1 Fig. 4 shows a flow schematic diagram of a UAV formation control method according to an embodiment of the present application. The specific implementation of the method can be seen with reference to Figure 2 the framework shown. The method comprises the following steps:

[0049] Step S110, taking each UAV in a UAV formation as a node, a virtual rigid structure is established, a virtual mass point of the rigid structure is calculated, and a reference coordinate system with the virtual mass point as the origin is established. A schematic diagram of the virtual mass point can be seen with reference to Figure 3 Fig. 5;

[0050] Step S120, each of the UAVs is projected onto the reference coordinate system, and the relative position between each of the UAVs and the virtual mass point is calculated;

[0051] Step S130, based on the current state of the virtual mass point, the current state of each of the UAVs, and the relative position, an actual formation configuration is determined;

[0052] Step S140, the difference between the expected formation configuration and the actual formation configuration is calculated, and based on the difference, the relative position of each of the UAVs and the virtual mass point at the next time is determined, and each of the UAVs is controlled based on the relative position at the next time. It should be noted that the ideal trajectory and the expected position of the virtual mass point can be preset in advance as needed, and the ideal formation configuration of the UAVs is also preset in advance, and can be changed as needed.

[0053] Specifically, with reference to Figure 2As shown, by inputting the desired formation configuration, the virtual mass point state and the current time state into the formation cooperative controller, the state of the virtual mass point and the real-time state of the unmanned aerial vehicle are solved by the relative position to obtain the actual formation configuration, and the formation error is obtained by subtracting the desired formation configuration, and input into the XY direction motion control to calculate the acceleration information of the unmanned aerial vehicle, and then control the inner loop of the unmanned aerial vehicle.

[0054] The unmanned aerial vehicle is a typical under-actuated system, so a hierarchical control strategy needs to be adopted. That is, the outer loop controls the position of the unmanned aerial vehicle in the formation, and the inner loop controls the attitude of the unmanned aerial vehicle. In the control process, the outer loop controller needs to provide effective information to the inner loop controller.

[0055] In summary, the embodiment adopts the virtual structure method to control the formation, and the core idea of the virtual structure method is to make each unmanned aerial vehicle follow a mass point on a moving rigid structure. This method can well avoid the problem that the entire formation is paralyzed when the leader of other formation control methods is damaged or cannot work normally. After the formation shape and the virtual mass point are determined, other unmanned aerial vehicles will follow the moving mass point, and the unmanned aerial vehicle can track the ideal trajectory of the mass point to maintain the structure of the formation. The above method can bring good rapidity and robustness to the unmanned aerial vehicle formation.

[0056] In some embodiments, the step S120 of projecting each of the unmanned aerial vehicles onto the reference coordinate system to calculate the relative position between each of the unmanned aerial vehicles and the virtual mass point specifically includes:

[0057] A first coordinate system is constructed to determine the kinematic model of the virtual mass point and each of the unmanned aerial vehicles in the height channel and the kinematic model of the virtual mass point and each of the unmanned aerial vehicles in the horizontal channel, and the kinematic model of the horizontal channel of the unmanned aerial vehicle includes external disturbances;

[0058] The relative distance between the unmanned aerial vehicle and the virtual mass point in the first coordinate system and the angle between the relative distance and the X-axis of the first coordinate system are determined, and based on the relative distance, the angle and the kinematic model, a relative position formula of each of the unmanned aerial vehicles relative to the virtual mass point is obtained.

[0059] Specifically, the virtual structure method in the present application forms a rigid structure among multiple unmanned aerial vehicles, and the unmanned aerial vehicles are nodes of the rigid structure, and the coordinate system on the rigid structure is taken as the home coordinate system (the first coordinate system). When the rigid structure moves, the coordinates of the unmanned aerial vehicles in the reference coordinate system do not change, and the relative positions of each of the unmanned aerial vehicles do not change, so that the effect of maintaining the formation shape is achieved. The positions of the virtual mass point and the unmanned aerial vehicles in the home coordinate system are as shown in the figure. Figure 3 e x e y e z e ​The coordinate system is a Home coordinate system, o b x b y b z b The coordinate system is a Local coordinate system. (x l , y l ) is the position of the virtual mass point in the Home coordinate system. Its yaw angle is ψ l . (x b , y b ) is the position of the unmanned aerial vehicle in the Home coordinate system. The target of the application is to make the unmanned aerial vehicle and the virtual mass point form a corresponding formation for flight, and make the yaw angle of the unmanned aerial vehicle consistent with the yaw angle of the virtual mass point. Further, the formation can be kept, transformed and dispersed.

[0060] In the formation control, the complete dynamics model of the quad-rotor unmanned aerial vehicle will not be considered, but only the kinematics model of the quad-rotor unmanned aerial vehicle. The kinematics model of the height channel of the quad-rotor unmanned aerial vehicle is

[0061]

[0062] Wherein i = l, f respectively represent the virtual mass point and the unmanned aerial vehicle, z represents the displacement of the quad-rotor unmanned aerial vehicle in the Home coordinate system, and v represents the speed of the quad-rotor unmanned aerial vehicle in the ground coordinate system.

[0063] The kinematics model of the horizontal position channel of the virtual mass point and the unmanned aerial vehicle can be established as

[0064]

[0065] Wherein, d i (i = x, y, z) is a disturbance term. Since the unmanned aerial vehicle is inevitably affected by external disturbances (wind disturbance, inter-aircraft airflow, etc.) when flying in formation, and the complete quad-rotor unmanned aerial vehicle mathematical model is not considered in the formation controller design, the disturbance term d is included in the kinematics model of the follower to approximately represent the influence of these external disturbances on the state of the quad-rotor unmanned aerial vehicle.

[0066] x, y are the horizontal positions in the Home coordinate system. v x , v y respectively represent the horizontal direction speed in the Local coordinate system. ψ, ω respectively represent the yaw angle in the Home coordinate system and the yaw angular velocity in the Local coordinate system, wherein i = l, f respectively represent the virtual mass point and the unmanned aerial vehicle.

[0067] The relative distance of the virtual mass point and the unmanned aerial vehicle in the Home coordinate system is l, and the angle with the X axis of the Home coordinate system is To establish the formation configuration around the virtual mass point, the relative positions between the followers and the virtual leader in the Local coordinate system of the virtual mass point need to be established and projected to the coordinate system of the virtual mass point (reference coordinate system) to obtain

[0068]

[0069] In some embodiments, in combination with Figure 2 As shown in the figure, the difference between the target formation configuration and the initial formation configuration is calculated, and the relative positions of each of the UAVs and the virtual mass point at the next moment are determined based on the difference, and the control of each of the UAVs based on the relative positions at the next moment includes:

[0070] The vertical direction force of each of the UAVs is determined based on the difference in the vertical direction in the relative positions at the next moment;

[0071] The attitude control parameters of each of the UAVs are determined based on the difference in the horizontal direction and the difference in the yaw angle in the relative positions at the next moment.

[0072] Specifically, in combination with Figure 2 As shown in the figure, the vertical direction force F of each of the UAVs and the attitude control parameters σ1, σ2, σ3, etc.

[0073] In some embodiments, the difference between the target formation configuration and the initial formation configuration is calculated, and the relative positions of each of the UAVs and the virtual mass point at the next moment are determined based on the difference, and the control of each of the UAVs based on the relative positions at the next moment further includes:

[0074] The action force parameters and / or attitude control parameters of each of the UAVs are determined based on the relative positions at the next moment by using a pre-constructed model compensation controller;

[0075] The model compensation controller is a controller based on the principle of active disturbance rejection control, including a high-order differentiator, a compensation function observer, and a model compensation control law.

[0076] The expected relative positions of the X and Y axes in the reference coordinate system of the virtual mass point, i.e., the expected formation configuration, are:

[0077]

[0078] Where l(t) is the offset of the UAV from the virtual mass point, and the orthogonal decomposition thereof obtains the formation configuration λ xd , λ yd . Therefore, only the controller needs to be designed to make λ x → λ xd , λ y → λ ydThe formation can be maintained.

[0079] According to the above formula, the expression formula of each unmanned aerial vehicle in the formation (which can be constructed in combination with the conversion formula in the specific embodiments described below) is introduced into the expression formula of each unmanned aerial vehicle, and then each subsystem in the expression formula of the unmanned aerial vehicle is described by a second-order differential equation in a nonlinear affine system

[0080]

[0081] where u is the input of the affine system, b is the system parameter, is the total disturbance of the system, is the output of the second-order differential equation. According to the above formula, a model compensation controller is designed to estimate the parameters therein.

[0082] It should be noted that the model compensation controller is a control strategy that aims to improve the performance of the control system by utilizing part of the mathematical model information of the controlled object. This controller design method fully utilizes the information of the known model to improve the dynamic response, anti-interference characteristics and robustness of the system. Specifically, the application of the model compensation controller involves the following aspects: high-order differentiator: a high-order differentiator is used in a control system to estimate the differential information of the system output or reference input. In some cases, such as the attitude control of a quadrotor, a high-order differentiator is used to improve the control algorithm to better handle the nonlinearity and unknown disturbances of the system. Compensated function observer (CFO): a compensated function observer is a tool for estimating unknown functions or disturbances of a system. Compared with the extended state observer (ESO), CFO has higher type, higher accuracy and stronger convergence. CFO improves the estimation ability of unknown parts of the system by changing the structure of ESO, adopting the ideas of pure integration, compensation and transfer function type, and further improves the estimation accuracy by combining advanced technologies such as neural networks. Model compensation control law: model compensation control law is a control strategy designed based on in-depth understanding of the system model. It uses the estimation information provided by the high-order differentiator and the compensated function observer to actively compensate the system to eliminate or reduce the influence of disturbances on the performance of the system. The design of the model compensation control law needs to consider the dynamic characteristics of the system, control objectives and constraint conditions, etc.

[0083] In some embodiments, the method further comprises:

[0084] determining a dynamic equation of the unmanned aerial vehicle based on the reference coordinate system and the body coordinate system;

[0085] determining a disturbance force formula and a disturbance torque formula that the unmanned aerial vehicle bears based on external disturbance force factors that the unmanned aerial vehicle is subjected to;

[0086] Based on the dynamic equation, the interference force formula and the interference torque, a relationship formula between the control torque and the force of the UAV and the propeller speed is determined;

[0087] The position system formula and attitude system formula of the UAV are determined based on the relationship formula.

[0088] It is assumed that the UAV is a rigid body, and the geometric center and center of mass remain unchanged. Define the inertial coordinate system O e =[x e ,y e , z e ] T (i.e. reference coordinate system) and the body coordinate system O b =[x b ,y b ,z b ] T , the positive directions defined by the two coordinates follow the right-hand rule. and are the position vector and velocity vector in the inertial coordinate system respectively. and are the angular velocity vector and three attitude angles (roll angle, pitch angle, yaw angle) in the body coordinate system respectively. The derivation and construction process of the formula can be referred to the following specific embodiment.

[0089] In some embodiments, the method further comprises:

[0090] Simplifying the position system formula and the attitude system formula to obtain simplified formulas;

[0091] The force parameter and / or posture control parameter are determined based on the simplified formula and the model compensation controller.

[0092] In some specific embodiments, in step S140, calculating the difference between the target formation configuration and the initial formation configuration, and determining the relative position of each of the UAVs and the virtual particle at the next moment based on the difference, and controlling each of the UAVs based on the relative position at the next moment further includes:

[0093] Combine Figure 3 As shown, the force and the attitude control parameters are converted and distributed to obtain the expected attitude signal required by the inner loop of each drone, and then determine the motor speed control value of each drone rotor.

[0094] According to this embodiment, the conversion of the control information of the inner ring of the drone is achieved based on the position information of the outer ring. Specific embodiments

[0096] The implementation process of the embodiment includes: obtaining state information of each aircraft, trajectory information of virtual center of mass, formation configuration information, calculating formation configuration error of each unmanned aerial vehicle; in the control part, the model compensation controller MCC control is used to obtain an acceleration signal, and the acceleration signal is converted into an expected attitude signal required by the unmanned aerial vehicle inner loop and then transmitted to the unmanned aerial vehicle through UDP communication. The unmanned aerial vehicle controls the inner loop according to the received expected attitude signal.

[0097] Specifically, establishing the mathematical model of the unmanned aerial vehicle is the basis for analysis and research. The unmanned aerial vehicle is modeled as shown in the four-rotor unmanned aerial vehicle. Figure 4

[0098] It is assumed that the unmanned aerial vehicle is a rigid body, and the geometric center and the center of mass remain unchanged. Define the inertial coordinate system O e = [x e , y e , z e ] T and the body coordinate system O b = [x b , y b , z b ] T The positive direction defined by the two coordinates complies with the right-hand rule. Let and be the position vector and velocity vector in the inertial coordinate system, respectively. and be the angular velocity vector and three attitude angles (roll angle, pitch angle, and yaw angle) in the body coordinate system, respectively. The rotation matrix from the body coordinate system O b to the inertial coordinate system O e is

[0099]

[0100] The dynamics equation of the four-rotor unmanned aerial vehicle is

[0101]

[0102] wherein,

[0103]

[0104] In the formula, m is the mass of the unmanned aerial vehicle, J = [J x , J y , J z ] T is the inertia matrix of the four-rotor unmanned aerial vehicle in O b along x b , y b , and z b ​The moment of inertia of the three coordinate axes. F = [0, 0, ui] T The total thrust, G = [0, 0, -mg] T The gravity, g is the acceleration of gravity, τ = [u2, u3, u4] T The control torque. D, d are the external disturbance force and disturbance torque, respectively. The external disturbance is divided into two parts, the drag of the quad-rotor aircraft in the three coordinate axes direction in O e and the drag torque in Ob. Random disturbance force and torque caused by other factors such as gust, unmodeled error, etc. It is assumed that the size of the drag and the drag torque is proportional to the corresponding speed, angular velocity, and the random disturbance force and torque satisfy the norm bounded condition, that is

[0105]

[0106] K i (i = 1, …, 6) represents the corresponding drag and drag torque coefficient, σ D = [σ1, σ2, σ3] T is the random disturbance force, σ d = [σ4, σ5, σ6] T is the random disturbance torque, ε D and ε d are the upper bounds of the random disturbance force and torque.

[0107] The unmanned aerial vehicle is an "X" type structure, and the total thrust and control torque are related to the propeller speed as follows:

[0108]

[0109] where represents the speed of the propeller, is the tension coefficient, is the torque coefficient, represents the distance from the center of the body to the motor.

[0110] In order to facilitate the design of the controller, the quad-rotor unmanned aerial vehicle dynamics model can be divided into two subsystems: position system Π1, attitude system Π2, as follows:

[0111]

[0112]

[0113] where

[0114]

[0115] Each subsystem in the position system Π1 and the attitude system Π2 can be described by a second-order differential equation in a nonlinear affine system

[0116]

[0117] where u is the input of the affine system, b is the system parameter, is the total disturbance of the system, is the output of the second order differential equation. The model compensation controller is designed according to the above equation, which consists of a high order differentiator, a compensation function observer, and a model compensation control law,

[0118] 1) Design HOD to track the reference signal as soon as possible and extract the high order differential of the reference signal.

[0119] The HOD design process is as follows

[0120]

[0121] where v i (i = 1, 2, 3) are the internal states of the HOD system, is the estimation of y r and its high order differential. Where

[0122]

[0123] a h is an adjustable parameter, which represents the bandwidth of the HOD. The larger the bandwidth, the higher the accuracy of the estimation of y r and its high order differential, but the greater the impact on the system.

[0124] 2) Design CFO to estimate the system state y, and the total disturbance f. The CFO design process is as follows

[0125]

[0126] where z i (i = 1, 2, 3) are the internal states of the CFO system. is the estimation of . Where

[0127]

[0128] a c is an adjustable parameter, which represents the bandwidth of the CFO. The larger the bandwidth, the higher the accuracy of the estimation of the state, but the greater the noise of the estimation.

[0129] 3) Design the model compensation control law according to the estimation of y r and its high order differential by the HOD and the estimation of The final design model compensator control law makes the system tracking error gradually converge to 0, i.e. when t→∞, (y r -y)→0.

[0130]

[0131] wherein a m >0 is an adjustable parameter.

[0132] Similarly, for each subsystem, the controller is designed in the same way.

[0133] In combination Figure 6 As shown in the figure, the application also protects a UAV formation control device 600, which comprises:

[0134] A particle calculation module 610 is adapted to take each UAV in the UAV formation as a node, establish a virtual rigid structure, calculate a virtual particle of the rigid structure, and establish a reference coordinate system with the virtual particle as the origin;

[0135] A position determination module 620 is adapted to project each UAV onto the reference coordinate system and calculate the relative position between each UAV and the virtual particle;

[0136] A formation determination module 630 is adapted to determine the actual formation configuration based on the current state of the virtual particle, the current state of each UAV, and the relative position;

[0137] A position control module 640 is adapted to calculate the difference between the desired formation configuration and the actual formation configuration, determine the relative position of each UAV and the virtual particle at the next time based on the difference, and control each UAV based on the relative position at the next time.

[0138] According to this embodiment, the virtual structure method is used to control the formation, and the core idea of the virtual structure method is to make each UAV follow a particle on a moving rigid structure. This device can well avoid the problem that the entire formation is paralyzed when the leader of other formation control methods is damaged or cannot work normally. After the formation configuration and the virtual particle are determined, other UAVs will follow the moving particle, and the UAVs can maintain the structure of the formation by tracking the ideal trajectory of the particle. In addition, the above device can also bring good rapidity and robustness to the UAV formation.

[0139] In some embodiments, the projection of each UAV onto the reference coordinate system in the position determination module 620 to calculate the relative position between each UAV and the virtual particle specifically comprises:

[0140] construct a first coordinate system, determine kinematic models of the virtual mass point and each of the UAVs in a height channel and a horizontal channel, and the kinematic model of the horizontal channel of the UAV includes external disturbances;

[0141] determine a relative distance between the UAVs and the virtual mass point in the first coordinate system and an angle between the relative distance and an X-axis of the first coordinate system, and obtain a relative position formula of each of the UAVs relative to the virtual mass point based on the relative distance, the angle, and the kinematic models.

[0142] In some embodiments, the position control module 640 calculates a difference between the target formation configuration and the initial formation configuration, determines a relative position of each of the UAVs and the virtual mass point at a next time based on the difference, and controls each of the UAVs based on the relative position at the next time, which includes:

[0143] determining an acting force of each of the UAVs in a vertical direction based on a height difference in the relative position at the next time;

[0144] determining an attitude control parameter of each of the UAVs based on a difference in a horizontal direction and a yaw angle difference in the relative position at the next time.

[0145] In some embodiments, the position control module 640 calculates a difference between the target formation configuration and the initial formation configuration, determines a relative position of each of the UAVs and the virtual mass point at a next time based on the difference, and controls each of the UAVs based on the relative position at the next time, which further includes:

[0146] determining an acting force parameter and / or an attitude control parameter of each of the UAVs based on the relative position at the next time by using a pre-constructed model compensation controller;

[0147] The model compensation controller is a controller based on a self-disturbance control principle, including a high-order differentiator, a compensation function observer, and a model compensation control law.

[0148] In some embodiments, the apparatus 600 is further adapted to:

[0149] determine a dynamic equation of the UAV based on the first coordinate system and a body coordinate system;

[0150] determine an interference force formula and an interference torque formula of the UAV based on external interference force factors to which the UAV is subjected;

[0151] determine a relationship formula between a control torque and an acting force of the UAV and a propeller rotation speed based on the dynamic equation, the interference force formula, and the interference torque;

[0152] determine a position system formula and a posture system formula of the UAV based on the relationship formula.

[0153] In some embodiments, the apparatus 600 is further adapted to:

[0154] simplify the position system formula and the posture system formula to obtain simplified formulas;

[0155] determine the force parameters and / or the posture control parameters based on the simplified formulas and the model compensation controller.

[0156] In some embodiments, the position control module 640 calculates a difference between the target formation configuration and the initial formation configuration, and determines a relative position between each of the UAVs and the virtual mass at a next time based on the difference, and the controlling each of the UAVs based on the relative position at the next time further comprises:

[0157] transform and distribute the force parameters and the posture control parameters to obtain expected posture signals required by inner loops of each of the UAVs, and further determine motor speed control values of rotors of each of the UAVs.

[0158] It should be noted that the specific embodiments of the above-mentioned apparatuses can refer to the specific embodiments of the corresponding method embodiments described above, and will not be described here again.

[0159] It should be noted that:

[0160] The algorithms and displays presented herein are not inherently related to any particular computer, virtual apparatus, or other apparatus. Various general-purpose systems can be used with these teachings, based on the description as set forth herein. In addition, the present application is not described with reference to any particular programming language. It will be appreciated that a variety of programming languages can be used to implement the teachings of the application as described herein, and any references below to specific languages are provided for disclosure of enablement of the best mode of the application.

[0161] In the description provided herein, numerous specific details are set forth. However, it is understood that embodiments of the application can be practiced without these specific details. In some instances, well-known methods, structures and techniques have not been described in detail in order to avoid obscuring the understanding of this description.

[0162] Similarly, it is to be understood that the embodiments of the present application can be alternately grouped together in a single embodiment, figure, or description of embodiments thereof, in order to streamline the disclosure and help understand one or more of the individual embodiments of the present application. However, this method of disclosure is not to be interpreted as reflecting an intention that the claimed application requires more features than are explicitly recited in each claim.

[0163] Those skilled in the art will appreciate that the modules in the apparatuses in the embodiments can be adaptively changed and arranged in one or more apparatuses different from the embodiments. The modules or units or components in the embodiments can be combined into one module or unit or component, and furthermore can be divided into multiple sub-modules or sub-units or sub-components. Except that at least some of such features and / or processes or units are mutually exclusive, any combination of all the features disclosed in the specification (including the accompanying claims, abstract and drawings), and all processes or units of any method or apparatus disclosed in the specification can be adopted. Unless explicitly stated otherwise, each feature disclosed in the specification (including the accompanying claims, abstract and drawings) can be replaced by an alternative feature providing the same, equivalent or similar function.

[0164] Furthermore, those skilled in the art will appreciate that a combination of features of different embodiments can mean being within the scope of the present application and forming a different embodiment.

[0165] The various component embodiments of the present application can be implemented in hardware, or as software modules running in one or more processors, or in combination thereof. Those skilled in the art will appreciate that a microprocessor or a digital signal processor (DSP) can be used in practice to implement some or all of the functions of some or all of the components in the unmanned aerial vehicle formation control apparatus according to the embodiments of the present application. The present application can also be implemented as a device or apparatus program (for example, a computer program and a computer program product) for executing part or all of the methods described herein. Such a program implementing the present application can be stored in a computer readable medium, or can be in the form of one or more signals. Such a signal can be downloaded from an Internet website, or provided on a carrier signal, or provided in any other form.

[0166] The embodiments of the present application provide a non-volatile computer storage medium, which stores at least one executable instruction, and the computer executable instruction can execute the unmanned aerial vehicle formation control method in any method embodiment described above.

[0167] Figure 7 A structural schematic diagram of an embodiment of the unmanned aerial vehicle of the present application is shown, and the embodiment of the present application does not limit the specific structure of the unmanned aerial vehicle.

[0168] As shown in Figure 7 The unmanned aerial vehicle (including the flight control system) can include a processor 702, a communications interface 704, a memory 706, and a communications bus 708.

[0169] The processor 702, the communications interface 704, and the memory 706 can communicate with each other through the communications bus 708. The communications interface 704 is configured to communicate with network elements such as clients or other servers. The processor 702 is configured to execute the program 710, and specifically can execute the related steps in the above unmanned aerial vehicle formation control method embodiments for the unmanned aerial vehicle.

[0170] Specifically, the program 710 can include program code including computer operation instructions.

[0171] The processor 702 can be a central processing unit CPU, or an application specific integrated circuit ASIC, or one or more integrated circuits configured to implement embodiments of the present application. The one or more processors included in the unmanned aerial vehicle can be the same type of processor, such as one or more CPUs; or can be different types of processors, such as one or more CPUs and one or more ASICs.

[0172] The memory 706 is configured to store the program 710. The memory 706 can include a high-speed RAM memory, and can also include a non-volatile memory such as at least one disk memory.

[0173] The program 710 can specifically be used to cause the processor 702 to perform the operations corresponding to the above unmanned aerial vehicle formation control method embodiments.

[0174] It should be noted that the above-mentioned embodiments illustrate rather than limit the application, and that those skilled in the art will be able to design many alternative embodiments without departing from the scope of the appended claims. In the claims, any reference signs placed between parentheses shall not be construed as limiting the claim. The word "comprising" does not exclude the presence of elements or steps not listed in a claim. The word "a" or "an" preceding an element does not exclude the presence of a plurality of such elements. The application can be implemented by means of both hardware and software, and any combination thereof. In a unitary claim, several devices or means can be listed, comprising means which can be implemented by one and the same hardware item. The use of the word "a" or "an" does not exclude the presence of a plurality of such elements, nor does it imply that a single element is to be used.

Claims

1. A UAV formation control method, characterized in that: The method comprises: Using each drone in the drone formation as a node, establishing a virtual rigid structure, calculating a virtual mass point of the rigid structure, and establishing a reference coordinate system with the virtual mass point as the origin; Projecting each of the drones onto the reference coordinate system, and calculating the relative position between each of the drones and the virtual mass point; determining an actual formation configuration based on the current state of the virtual particle, the current state of each of the drones, and the relative positions; The difference between the expected formation configuration and the actual formation configuration is calculated, and the relative position of each UAV and the virtual particle at the next moment is determined based on the difference, and each UAV is controlled based on the relative position at the next moment.

2. The method according to claim 1, characterized in that Projecting each of the drones onto the reference coordinate system and calculating the relative position between each of the drones and the virtual mass point specifically includes: Constructing a first coordinate system, determining a kinematic model of the virtual mass point and each of the drones in a height channel and a kinematic model of a horizontal channel, wherein the kinematic model of the horizontal channel of the drone includes external disturbances; Determine the relative distance between the UAV and the virtual mass point in the first coordinate system, and the angle between the relative distance and the X-axis of the first coordinate system. Based on the relative distance, the angle, and the kinematic model, obtain a relative position formula for each UAV relative to the virtual mass point.

3. The method according to claim 1, characterized in that Calculating a difference between the target formation configuration and the initial formation configuration, determining a relative position of each of the UAVs and the virtual particle at a next moment based on the difference, and controlling each of the UAVs based on the relative position at the next moment includes: Determining the vertical force of each of the drones based on the height difference in the relative positions at the next moment; The attitude control parameters of each of the UAVs are determined based on the horizontal difference and the yaw angle difference in the relative positions at the next moment.

4. The method according to claim 3, characterized in that Calculating a difference between the target formation configuration and the initial formation configuration, and determining a relative position of each of the UAVs and the virtual particle at a next moment based on the difference, and controlling each of the UAVs based on the relative position at the next moment further includes: Determining force parameters and / or attitude control parameters on each of the UAVs based on the relative position at the next moment using a pre-built model compensation controller; The model compensation controller is a controller based on the active disturbance rejection control principle, including a high-order differentiator, a compensation function observer and a model compensation control law.

5. The method according to claim 4, characterized in that The method further comprises: Determining a dynamic equation of the UAV based on a reference coordinate system and a body coordinate system; Determine the interference force formula and interference torque formula borne by the UAV based on the external interference force factors to which the UAV is subjected; Based on the dynamic equation, the interference force formula and the interference torque, a relationship formula between the control torque and the force of the UAV and the propeller speed is determined; The position system formula and attitude system formula of the UAV are determined based on the relationship formula.

6. The method according to claim 5, characterized in that The method further comprises: Simplifying the position system formula and the attitude system formula to obtain simplified formulas; The force parameter and / or posture control parameter are determined based on the simplified formula and the model compensation controller.

7. The method according to any one of claims 2 to 6, characterized in that Calculating a difference between the target formation configuration and the initial formation configuration, and determining a relative position of each of the UAVs and the virtual particle at a next moment based on the difference, and controlling each of the UAVs based on the relative position at the next moment further includes: The acting force and the attitude control parameters are converted and distributed to obtain the desired attitude signal required by the inner loop of each UAV, and then determine the motor speed control value of each UAV rotor.

8. A UAV formation control device, characterized in that: The device comprises: a mass point calculation module adapted to establish a virtual rigid structure using each drone in the drone formation as a node, calculate a virtual mass point of the rigid structure, and establish a reference coordinate system with the virtual mass point as the origin; a position determination module, adapted to project each of the drones onto the reference coordinate system and calculate the relative position between each of the drones and the virtual mass point; a formation determination module, adapted to determine an actual formation configuration based on a current state of the virtual particle, a current state of each of the UAVs, and the relative positions; The position control module is adapted to calculate the difference between the expected formation configuration and the actual formation configuration, determine the relative position of each of the UAVs and the virtual particle at a next moment based on the difference, and control each of the UAVs based on the relative position at the next moment.

9. A drone, characterized in that: The system comprises a processor and a memory arranged to store computer-executable instructions, wherein when the executable instructions are executed, the processor executes the drone formation control method according to any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that The computer-readable storage medium stores one or more programs, which, when executed by a processor, implement the drone formation control method according to any one of claims 1 to 7.

Images