A UAV vector formation cooperative control method and system

Abstract

The present invention discloses a method and system for cooperative control of UAV vector formation, which relates to the technical field of UAV control. The method includes constructing a UAV formation and defining a UAV set based on the predator-prey mechanism, dynamically adjusting the speed and position of the UAVs according to the predator-prey mechanism, and using the artificial potential field method to optimize and update the speed and position of the UAVs at the next moment. By defining the predator set and the prey set of the UAVs, the method of the present invention can dynamically adjust the formation structure according to the mutual relationship between the UAVs and the surrounding neighboring UAVs. Through the combination of the predator-prey mechanism and the artificial potential field method, multi-level optimization is achieved, ensuring that the UAVs can maintain stability in the local formation. The predation intensity mechanism adjusts through the relative distance to ensure the reasonable position of the UAVs in the formation. By combining the Kalman filter with the INS system, the cumulative error can be periodically corrected using external sensors.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned aerial vehicle control, and in particular to a method and system for coordinated control of an unmanned aerial vehicle vector formation. Background Art

[0002] UAV vector formation is a technology that controls the speed, direction and position of multiple UAVs to make them fly in a predetermined shape and path in three-dimensional space. Similar to the traditional UAV formation control method, UAV vector formation still relies on formation maintenance and position coordination, but the control method is more precise and flexible. The core of UAV formation technology is to complete tasks that are difficult for a single UAV to achieve through the coordinated cooperation of multiple UAVs, such as large-area monitoring, complex environment reconnaissance, and precise logistics transportation.

[0003] Traditional UAV formation control methods mostly adopt a strategy based on centralized control, that is, a single central controller coordinates the flight paths and mission execution of multiple UAVs. However, with the expansion of the formation size and the increase of environmental complexity, traditional formation control often has a lag in response when dealing with emergencies in dynamic environments. It is difficult to adjust the speed and position of the UAV in real time, resulting in the formation structure being easily disrupted or colliding, and the inability to maintain the stability of the formation, which reduces the flexibility of UAVs in coping with collaborative control. Summary of the invention

[0004] In view of the problems existing in the above-mentioned existing UAV vector formation collaborative control method and system, the present invention is proposed.

[0005] Therefore, the problem to be solved by the present invention is that as the size of the formation increases and the complexity of the environment increases, traditional formation control often has a delayed response when dealing with emergencies in a dynamic environment, and it is difficult to adjust the speed and position of the UAV in real time, resulting in the formation structure being easily disrupted or colliding, and the stability of the formation cannot be maintained, thereby reducing the flexibility of the UAV in coping with collaborative control.

[0006] In order to solve the above technical problems, the present invention provides the following technical solutions: a method for coordinated control of a UAV vector formation, comprising:

[0007] Construct a UAV formation and define a UAV set based on the predator-escape mechanism. Dynamically adjust the speed and position of the UAV according to the predator-escape mechanism. Use the artificial potential field method to optimize and update the speed and position of the UAV at the next moment.

[0008] Deploy the INS inertial navigation system, cooperate with sensors to obtain the status information of the drone, use the Kalman filter to correct the status information of the drone, input the corrected drone status information into the drone, and continuously correct it according to the drone status information at the next moment;

[0009] Securely transmit the flight data of the drones and authenticate the drone formation.

[0010] As a preferred embodiment of the drone vector formation cooperative control method of the present invention, wherein: the construction of the drone formation and the definition of the drone set based on the predator-prey mechanism includes,

[0011] Construct a drone formation according to the number of drones, initialize the states of the drones, and define the drone states including the position of the i-th drone at time t and the velocity of the i-th drone at time t ;

[0012] Define the predator and escape drones of the drones based on the predator-prey mechanism, and initialize the predation intensity, where the set of predator drones is , representing the set of neighbor drones of drone i, and the set of escape drones is , representing the set of non-neighbor drones of drone i.

[0013] As a preferred embodiment of the drone vector formation cooperative control method of the present invention, wherein: the dynamic adjustment of the drone velocity and position according to the predator-prey mechanism, and the use of the artificial potential field method to optimize and update the drone velocity and position at the next moment includes,

[0014] Determine the optimal distance between drones based on the communication distance of the drones, the detection range of the sensors, and the physical size of the drones, and divide the optimal distance between drones by the actual distance between the current drones and then multiply by the current predation intensity of the predator drones to determine the adjusted predation intensity at the next moment;

[0015] Adjust the drone velocity through the predator-prey mechanism based on the distances between the set of predator drones and the set of escape drones and drone i, expressed as:

[0016] ;

[0017] where represents the velocity of drone i at the next moment t + 1, represents the current velocity of drone i, , and represent the positions of drones j, i, and k respectively, where drone j belongs to the set of predator drones and drone k belongs to the set of escape drones, represents the predation intensity of drone i at the next moment t + 1, represents the set of predator drones, represents the set of escape drones;

[0018] Based on the speed of UAV i at the next moment t+1, the position of the UAV at the current moment is adjusted by multiplying the time step by the speed of the UAV at the next moment, and the position of the UAV is updated;

[0019] Based on the position of the UAV at the next moment, the distance between the UAV and the target position is updated according to the target position of the UAV , and based on the maximum acceleration of the UAV divided by Calculate the gravitational constant;

[0020] Based on the maximum flight speed of the UAV divided by the maximum deceleration speed, the obstacle avoidance response time of the UAV is obtained. According to the obstacle avoidance response time multiplied by the maximum speed of the UAV plus one-half of the maximum deceleration of the UAV multiplied by the square of the obstacle avoidance response time, the safety distance between the UAV and the obstacle is obtained;

[0021] Based on the mass of the UAV multiplied by the minimum obstacle avoidance acceleration of the UAV during obstacle avoidance and multiplied by the square of the safety distance between obstacles, the repulsive force constant is calculated;

[0022] According to the artificial potential field method, the gravitational gradient and the repulsive force gradient are calculated, expressed as:

[0023] ;

[0024] ;

[0025] Among them, represents the gravitational gradient of the position of the i-th UAV at the next moment, represents the gravitational constant, represents the target position of the UAV, represents the repulsive force gradient of UAV i at time t+1, represents the repulsive force constant, represents the position of obstacle o, represents the safety distance between the UAV and the obstacle;

[0026] The sum of the gravitational gradient and the repulsive force gradient of UAV i at the next moment t+1 is used as the total resultant force, divided by the mass of the UAV, and the sum with is used as the final adjustment speed of UAV i at the next moment t+1, and the adjustment position of the UAV at the next moment t+1 is updated.

[0027] As a preferred scheme of the UAV vector formation cooperative control method described in the present invention, among them: the INS inertial navigation system is deployed, and the sensor is used to obtain the UAV state information, including,

[0028] Based on the deployment of the INS inertial navigation system on the UAV, the state information of the real-time position, speed, acceleration and attitude of the UAV is collected through the sensor;

[0029] Among them, the acceleration of the UAV in three-dimensional directions is measured by an acceleration sensor, and the gravitational acceleration is subtracted based on the earth's gravity to obtain the linear acceleration data of the UAV;

[0030] The angular velocity of the UAV in three-dimensional space is measured by a gyroscope. By integrating the linear acceleration, the velocity of the UAV is calculated based on the time step, and the attitude of the UAV is represented by Euler angles. The calculated velocity of the UAV is mapped to the global coordinate system.

[0031] As a preferred solution of the UAV vector formation cooperative control method described in the present invention, wherein: the use of a Kalman filter to correct the state information of the UAV includes,

[0032] Based on the data collected by INS inertial navigation and calculated data including acceleration, velocity, angular velocity, and position data, a state vector of the UAV is constructed;

[0033] Based on the predicted velocity and position of the UAV by the predator-prey escape mechanism and the artificial potential field method, the state of the UAV is corrected. Based on the sensors included in INS inertial navigation, the actual position and velocity data of the UAV at the same moment are obtained;

[0034] Based on the position, velocity, and acceleration data of the UAV, a state vector is constructed, and based on the state vector, the error covariance between position, velocity, and acceleration is constructed to form a covariance matrix ;

[0035] Based on the position, velocity, and acceleration data in the state vector, all are updated to the next data based on the time step to construct a state transition matrix A;

[0036] Based on the historical data of the sensors of the INS system, according to the error generated by the change of the UAV state over time, a process noise matrix Q is constructed;

[0037] Based on the covariance matrix , the process noise matrix Q, and the state transition matrix A, the covariance matrix is updated, expressed as:

[0038] ;

[0039] Wherein represents the updated covariance matrix, represents the transpose of the state transition matrix;

[0040] Based on the state vector of the UAV, an observation matrix H is constructed, and based on the error range value of the used sensor, an observation noise matrix R is constructed;

[0041] Use the Extended Kalman Filter (EKF) to balance the Kalman gain weight between the current predicted value and the observed value through the Kalman gain ;

[0042] Update the attitude of the UAV based on the Euler angles, time step, and angular velocity value, expressed as:

[0043] ;

[0044] where represents the Euler angle of UAV i at time t, represents the Euler angle of UAV i at time t + 1, represents the time step, represents the angular velocity of UAV i at time t;

[0045] Use the Kalman filter to construct the predicted state vector of the UAV based on the predicted UAV speed, position, and Euler angle values predicted by the predator-prey escape mechanism and the artificial potential field method ;

[0046] Based on the calculation of the Kalman gain, correct the state prediction value based on the predator-prey escape mechanism and the artificial potential field method, expressed as:

[0047] ;

[0048] where represents the corrected state vector, represents the predicted state vector, represents the actual value of the state vector;

[0049] Based on the corrected state vector, update the covariance matrix and perform state correction for the next time step through the Kalman filter.

[0050] As a preferred scheme of the UAV vector formation cooperative control method described in the present invention, wherein: the corrected UAV inputs the state information into the UAV and performs continuous correction according to the UAV state information at the next moment, including,

[0051] Apply the corrected state to the actual UAV navigation, and generate corresponding UAV control input data through the PID controller according to the corrected state vector;

[0052] Use the UAV control input data to be transmitted to the actuator of the UAV to control the UAV;

[0053] After the UAV executes the UAV control input data, the updated state data forms the state vector at the next moment, and is corrected again through the INS system and the Kalman filter to form a closed-loop correction.

[0054] As a preferred solution of the UAV vector formation cooperative control method of the present invention, wherein: securely transmitting the flight data of the UAVs and authenticating the UAV formation includes,

[0055] For the UAVs that have completed the flight, use the AES encryption algorithm to securely encrypt the flight data of the UAVs, and use the same key to decrypt the encrypted data for secure transmission when receiving the data;

[0056] The UAV formation uses the ECC asymmetric encryption algorithm for key exchange to authenticate the formation UAVs.

[0057] Another object of the present invention is to provide a system for the UAV vector formation cooperative control method, which includes,

[0058] The UAV formation module initializes the state of each UAV, including position and speed, and constructs the initial formation structure of the UAVs;

[0059] The speed and position update module initializes the UAV formation, and dynamically adjusts the position and speed of the UAVs based on the predator-prey escape mechanism, and further optimizes and updates the path and speed of the UAVs based on the artificial potential field method;

[0060] The INS inertial navigation module obtains the real-time position information, speed, acceleration, and attitude data of the UAVs through sensors such as accelerometers and gyroscopes;

[0061] The state correction module uses the Kalman filter to correct the UAV state data obtained by the INS system;

[0062] The data transmission module encrypts and transmits the data in the UAV formation, and authenticates the UAVs to ensure the security of data transmission and the authentication of the UAVs in the team.

[0063] A computer device includes: a memory and a processor; the memory stores a computer program, and when the processor executes the computer program, the steps of the above-mentioned UAV vector formation cooperative control method are implemented.

[0064] A computer-readable storage medium stores a computer program thereon, and when the computer program is executed by a processor, the steps of the above-mentioned UAV vector formation cooperative control method are implemented.

[0065] The beneficial effects of the present invention are as follows: By defining the predator set and escape set of drones, the formation structure can be dynamically adjusted according to the mutual relationship between the drone and the surrounding neighboring drones. Through the combination of the predator-prey escape mechanism and the artificial potential field method, multi-level optimization is achieved, ensuring that the drones can maintain stability in the local formation. The predation intensity mechanism adjusts through the relative distance to ensure the reasonable position of the drones in the formation. By using the Kalman filter in combination with the INS system, the cumulative error can be periodically corrected using external sensors. BRIEF DESCRIPTION OF THE DRAWINGS

[0066] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings required for the description of the embodiments will be briefly introduced below. Obviously, the drawings in the following description are only some embodiments of the present invention. For those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative efforts.

[0067] Figure 1 It is a schematic flowchart of the method for cooperative control of vector formation of drones.

[0068] Figure 2 It is a schematic structural diagram of the system for cooperative control of vector formation of drones. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0069] In order to make the above objects, features, and advantages of the present invention more obvious and understandable, the specific embodiments of the present invention will be described in detail below with reference to the drawings of the specification.

[0070] Many specific details are set forth in the following description in order to provide a thorough understanding of the present invention. However, the present invention may be practiced in other ways different from those described herein. Those skilled in the art can make similar extensions without departing from the spirit of the present invention. Therefore, the present invention is not limited by the specific embodiments disclosed below.

[0071] Secondly, the so-called "one embodiment" or "embodiment" herein refers to a specific feature, structure, or characteristic that can be included in at least one implementation manner of the present invention. The appearances of "in one embodiment" in different places in this specification do not all refer to the same embodiment, nor are they separate or selectively exclusive embodiments from other embodiments.

[0072] Embodiment 1, referring to Figure 1 , which is the first embodiment of the present invention. This embodiment provides a method for cooperative control of vector formation of drones. The method for cooperative control of vector formation of drones includes

[0073] S1. Construct a UAV formation and define a UAV set based on the predator-prey escape mechanism. Dynamically adjust the speed and position of the UAVs according to the predator-prey escape mechanism, and optimize and update the speed and position of the UAVs at the next moment by referring to the artificial potential field method.

[0074] Preferably, constructing a UAV formation and defining a UAV set based on the predator-prey escape mechanism includes:

[0075] Construct a UAV formation according to the number of UAVs, initialize the state of the UAVs, and define the UAV state including the position of the i-th UAV at time t and the speed of the i-th UAV at time t. ;

[0076] Define the predator and escape machines of the UAVs based on the predator-prey escape mechanism, and initialize the predation intensity. The predator machine set is , representing the set of neighbor UAVs of UAV i, and the escape machine set is , representing the set of non-neighbor UAVs of UAV i.

[0077] By defining the predator machine set and escape machine set of the UAVs, the formation structure can be dynamically adjusted according to the mutual relationship between the UAVs and the surrounding neighboring UAVs, ensuring the flexibility and dynamic adjustment ability of the UAV formation, enabling the UAVs to make real-time adjustments according to different mission requirements in a complex environment. Through the predator-prey escape mechanism, the formation can make adaptive adjustments according to the dynamic changes of the external environment and the mission, maintaining a flexible formation shape. By dividing the UAVs into predator machines and escape machines, the system can make adaptive adjustments according to the relative distance of the UAVs. When the distance between UAVs is too close, the escape mechanism enables the UAVs to avoid other UAVs that are too close in time, thus preventing collisions. When the distance is moderate, the predator mechanism makes the UAVs tend to their neighbors, maintaining a reasonable formation structure. Through the initialization setting of the predation intensity, the UAVs can automatically form a formation according to the distance and the predator-prey escape mechanism, and dynamically adjust the formation shape.

[0078] Furthermore, dynamically adjusting the speed and position of the UAVs according to the predator-prey escape mechanism, and optimizing and updating the speed and position of the UAVs at the next moment by referring to the artificial potential field method includes:

[0079] Determine the optimal distance between UAVs based on the communication distance of the UAVs, the detection range of the sensors, and the physical size of the UAVs. Divide the optimal distance between UAVs by the actual distance between the current UAVs and then multiply by the current predation intensity of the predator machine to determine the adjusted predation intensity at the next moment. Dynamically adjust the predation intensity according to the distribution of the UAVs, expressed as:

[0080] ;

[0081] ;

[0082] Wherein represents the predation intensity of UAV i at the next moment t + 1, represents the predation intensity of UAV i at moment t, represents the distance between UAV j and i, in meters (m), represents the optimal distance between UAVs, represents the communication effective radius of the UAV, represents the sensing range of the UAV sensor, and r represents the physical radius of the UAV;

[0083] Based on the distances between the predator set and the escapee set and UAV i, the speed of the UAV is adjusted through the predation - escape mechanism, expressed as:

[0084] ;

[0085] Wherein represents the speed of UAV i at the next moment t + 1, represents the current speed of UAV i, , and represent the positions of UAVs j, i, and k respectively, where UAV j belongs to the predator set and UAV k belongs to the escapee set, represents the predation intensity of UAV i at the next moment t + 1, represents the predator set, represents the escapee set;

[0086] In the formula, the calculation of the first term is the predation behavior between UAV i and UAV j, representing the direction and amplitude of UAV i being attracted by neighboring UAVs. The closer the neighboring UAV is, the greater its influence; the calculation of the second term is the escape behavior between UAV i and UAV k, representing the direction and amplitude of UAV i avoiding and moving away from the escapee set according to the positions of the escapee set, and the farther the UAV is, the smaller its influence;

[0087] Based on the speed of UAV i at the next moment t + 1, the position of the UAV at the current moment is adjusted by multiplying the time step and the speed of the UAV at the next moment, and the position of the UAV is updated, expressed as:

[0088] ;

[0089] Wherein represents the position of UAV i at the next moment t + 1, represents the position of the UAV at the current moment t, is the time step, representing the length of each time step in continuous time, with the unit of seconds (s);

[0090] Based on the position of the UAV at the next moment, update the distance between the UAV and the target position according to the target position of the UAV , and divide by the maximum acceleration of the UAV to calculate the gravitational constant;

[0091] The obstacle avoidance response time of the UAV is obtained by dividing the maximum flight speed of the UAV by the maximum deceleration speed. According to the obstacle avoidance response time multiplied by the maximum speed of the UAV plus half of the maximum deceleration of the UAV multiplied by the square of the obstacle avoidance response time, the safety distance between the UAV and the obstacle is obtained;

[0092] In the process of calculating the safety distance between the UAV and the obstacle, the driving distance within the reaction time calculated by multiplying the obstacle avoidance response time by the maximum speed of the UAV, and the content calculated by half of the maximum deceleration of the UAV multiplied by the square of the obstacle avoidance response time represents the additional driving distance of the UAV during deceleration; the sum of the two parts constitutes the minimum safety distance of the UAV to ensure that there is enough time and distance for obstacle avoidance operations after detecting an obstacle;

[0093] Calculate the repulsive constant based on the mass of the UAV multiplied by the minimum obstacle avoidance acceleration of the UAV during obstacle avoidance and multiplied by the square of the safety distance between obstacles;

[0094] By dividing the maximum speed of the UAV by the distance between the UAV and the target position, the calculated gravitational constant. When the UAV is far from the target point, the formula calculates a smaller gravitational constant, while when the UAV approaches the target point, the gravitational constant will become larger, which conforms to the basic principle of the gravitational field changing with distance in physics, and ensures that the UAV accelerates or decelerates according to the distance from the target, making the path planning of the UAV more dynamically adaptable;

[0095] The repulsive constant calculated based on the safety distance between the UAV and the obstacle, the minimum obstacle avoidance acceleration, and the mass of the UAV conforms to the classical mechanics formula ( ), and ensures that in different environments, regardless of the distance of the obstacle, the UAV can effectively generate a repulsive force, and the use of the repulsive constant and substituting it into the calculation of the repulsive gradient can effectively maintain the safety distance position of the UAV. For the calculation of the safety distance between the UAV and the obstacle, the speed, response time, and deceleration of the UAV are combined, dynamically considering the current speed and reaction time of the UAV, ensuring that even during high-speed flight, the UAV can avoid obstacles in time and maintain a safe distance, ensuring the robustness and safety of the UAV system;

[0096] Calculate the gravitational gradient and repulsive gradient according to the artificial potential field method, expressed as:

[0097] ;

[0098] ;

[0099] Wherein, represents the gravitational gradient of the i-th drone's position at the next moment (indicating the direction and magnitude of the drone's movement from the next moment's position towards ), represents the gravitational constant, represents the target position of the drone, represents the repulsive gradient of the i-th drone at time t + 1. This gradient indicates how the drone moves based on the distance from obstacles or other drones to avoid collisions, represents the repulsive constant, represents the position of obstacle o, represents the safe distance between the drone and the obstacle;

[0100] The sum of the gravitational gradient and the repulsive gradient of the i-th drone at the next moment t + 1 is used as the total resultant force, divided by the mass of the drone, and The sum is used as the final adjustment speed of the i-th drone at the next moment t + 1, and the adjusted position of the drone at the next moment t + 1 is updated.

[0101] The calculation of the gravitational gradient is based on the vector difference between the target point and the current position of the drone, such that the gravity increases as the target distance increases. When the drone is far from the target, the greater gravity pushes the drone to accelerate towards the target, and when the drone approaches the target, the smaller gravity ensures that the drone gradually decelerates to avoid overshooting, ensuring the flight dynamics of the drone at different distances;

[0102] The repulsive gradient formula is based on the inverse square relationship of distance, similar to the Coulomb force or gravitational formula in physics, ensuring that when the drone approaches an obstacle, the repulsive force increases sharply, forcing the drone to quickly move away from the obstacle, and when the distance from the obstacle is far, the repulsive force quickly decreases to zero, ensuring the locality and efficiency of the repulsive force, and by setting a safe distance, ensuring that the drone only generates a repulsive force when approaching an obstacle and is not affected by the repulsive force when far from the obstacle;

[0103] Through the calculation of the total resultant force, the drone can simultaneously benefit from the guiding effect of gravity and the obstacle avoidance function of the repulsive force. Gravity prompts the drone to move towards the target point, while the repulsive force ensures that the drone does not collide with obstacles, ensuring that the path optimization of the drone is not only to move towards the target point but also to adapt to the obstacle avoidance requirements in a dynamic environment, making the drone more flexible in a complex environment;

[0104] Through the UAV formation, the predation intensity can be adaptively adjusted according to the initial position, optimal distance and actual distance of each UAV. By dynamically updating the speed and position, the UAVs can achieve mutual coordination and formation maintenance. The adaptive adjustment based on the predation and escape mechanism avoids the complexity of a large number of manually set parameters in traditional formation control. The system automatically optimizes the flight strategy according to the actual state in the formation;

[0105] Among them, through the adaptive adjustment formula of the predation intensity, the UAV can dynamically adjust the attraction according to the actual distance from the neighboring UAVs, ensuring that the formation maintains the best state in various environments and mission scenarios. Compared with the traditional fixed predation intensity model, it can be more effectively applied to the complex usage environment of UAVs. By combining the predation intensity with the optimal distance, the UAVs can automatically adjust their relative positions under different mission requirements, thereby dynamically optimizing the path. At the same time, through the escape mechanism, the UAVs can avoid overly dense neighboring UAVs and obstacles to prevent collisions. Through this dynamic adjustment, the flexibility of the formation is maintained, and it can also adapt to dynamic external environments, such as suddenly appearing obstacles and sudden changes in mission requirements;

[0106] By introducing the artificial potential field method, the UAVs are further guided towards the target point by gravity and are ensured to avoid obstacles or other UAVs by repulsion to avoid collisions. This mechanism ensures that the UAVs can autonomously avoid obstacles in a complex environment while maintaining an effective path towards the target;

[0107] At the same time, through the combination of the predation and escape mechanism and the artificial potential field method, multi-level optimization is achieved, ensuring that the UAVs can not only maintain stability in the local formation but also globally optimize the path and obstacle avoidance ability. The predation intensity mechanism adjusts the relative distance to ensure the reasonable position of the UAVs in the formation. The artificial potential field method further adjusts the flight direction and speed of the UAVs, optimizes their paths towards the target point, and ensures collision avoidance in a complex environment, achieving the effect that the UAVs can adjust their relative positions while realizing the global path optimization and obstacle avoidance function through the artificial potential field method, enabling the UAV vector formation to fly along the best path without relying on external intervention, achieving efficient and stable path planning.

[0108] S2. Deploy the INS inertial navigation system, cooperate with sensors to obtain the UAV state information, use the Kalman filter to correct the UAV state information, input the corrected UAV state information into the UAV, and continuously correct according to the UAV state information at the next moment;

[0109] Preferably, deploy the INS inertial navigation system, cooperate with sensors to obtain the UAV state information, including,

[0110] Deploy an INS inertial navigation system based on an unmanned aerial vehicle (UAV), and collect the state information of the real-time position, speed, acceleration, and attitude of the UAV through sensors;

[0111] Among them, measure the acceleration of the UAV in three-dimensional directions by an acceleration sensor, and subtract the gravitational acceleration based on the earth's gravity to obtain the linear acceleration data of the UAV;

[0112] Measure the angular velocity of the UAV in three-dimensional space by a gyroscope, calculate the speed of the UAV based on the time step by integrating the linear acceleration, and represent the attitude of the UAV according to the Euler angles, and map the calculated speed of the UAV to the global coordinate system.

[0113] By performing gravity compensation on the measurement results of the acceleration sensor, the real linear acceleration of the UAV can be obtained. Then, by integrating the linear acceleration, the speed of the UAV can be updated in real time, and by further integrating the speed, the real-time position of the UAV in three-dimensional space can be calculated, ensuring that the UAV can always master its accurate position when performing tasks. By using the INS system, without relying on external signals, the UAV can still perform navigation through its own inertial sensors. By performing gravity compensation on the acceleration sensor and measuring the angular velocity of the gyroscope in the INS system, the influence of gravity on the acceleration of the UAV is further eliminated, thereby obtaining a more accurate linear acceleration. By using the Euler angles to map the speed of the UAV to the global coordinate system, the UAV can better adapt to the navigation requirements of complex three-dimensional spaces, and by mapping the attitude and speed to the global coordinate system, the UAV can adapt to path planning and control tasks in complex three-dimensional spaces.

[0114] Furthermore, use a Kalman filter to correct the state information of the UAV, including,

[0115] Construct the state vector of the UAV based on the data collected by the INS inertial navigation and the calculated data including acceleration, speed, angular velocity, and position data;

[0116] Based on the predicted speed and position of the UAV by the predator-prey escape mechanism and the artificial potential field method, perform state correction of the UAV, and obtain the actual position and speed data of the UAV at the same moment based on the sensors included in the INS inertial navigation;

[0117] Construct the state vector based on the position, speed, and acceleration data of the UAV, and construct the error covariance between the position, speed, and acceleration based on the state vector to form the covariance matrix , expressed as:

[0118] ;

[0119] where 、 and represent the variances of position, velocity, and acceleration respectively, , , , , and represent the covariance terms of every two items of position, velocity, and acceleration respectively;

[0120] Based on the fact that the position, velocity, and acceleration data in the state vector are all updated to the next data based on the time step to construct the state transition matrix A, which is expressed as:

[0121] ;

[0122] The first row of the matrix indicates that the position data at the next moment t + 1 is obtained by updating the current position, velocity, and acceleration through the time step. The second row indicates that the current velocity and acceleration are updated through the time step. The third row is based on the fact that the acceleration is usually regarded as a constant and thus does not change. The matrix represents the relationship between position, velocity, and acceleration as a function of time;

[0123] Based on the sensor historical data of the INS system, according to the errors generated by the time-varying state of the UAV, construct the process noise matrix Q, which is expressed as:

[0124] ;

[0125] where , and represent the state change error noises of position, velocity, and acceleration respectively;

[0126] Based on the covariance matrix update the covariance matrix based on the process noise matrix Q and the state transition matrix A, which is expressed as:

[0127] ;

[0128] where represents the updated covariance matrix, represents the transpose of the state transition matrix;

[0129] Based on the state vector of the UAV, construct the observation matrix H, which is expressed as:

[0130] ;

[0131] Based on the error range values of the sensors used, construct the observation noise matrix R, which is expressed as:

[0132] ;

[0133] Among them 、 and respectively represent the detection error noises of position, speed, and acceleration;

[0134] Use the Kalman filter EKF to balance the Kalman gain weight between the current predicted value and the observed value through the Kalman gain , expressed as:

[0135] ;

[0136] Among them represents the Kalman gain weight, represents the covariance matrix, represents the transpose calculation of the observation matrix, and R represents the observation noise matrix;

[0137] Based on the Euler angles at the time step and the angular velocity value, update the attitude of the UAV, expressed as:

[0138] ;

[0139] Among them represents the Euler angle of the UAV at time t, represents the Euler angle of the UAV at time t + 1, represents the time step, represents the angular velocity of the UAV at time t;

[0140] Use the Kalman filter to construct the predicted state vector of the UAV based on the predicted speed and position of the UAV and the predicted Euler angle value by the predator-prey escape mechanism and the artificial potential field method , expressed as:

[0141] ;

[0142] Based on the calculation of the Kalman gain, correct the state prediction value based on the predator-prey escape mechanism and the artificial potential field method, expressed as:

[0143] ;

[0144] Among them represents the corrected state vector, represents the predicted state vector, represents the actual value of the state vector;

[0145] Based on the corrected state vector, update the covariance matrix and perform state correction for the next time step through the Kalman filter.

[0146] The INS system collects real-time position, velocity, acceleration, and angular velocity data of the UAV. Based on the state prediction and correction mechanism of the Kalman filter, the system can significantly improve the state estimation accuracy of the UAV. Among them, the possible cumulative errors of the INS system are periodically corrected through external sensor data to ensure that the state estimation accuracy remains at a high level during long-term flight. By dynamically updating the covariance matrix and combining the use of the observation noise matrix and the process noise matrix, the interference of noise on state estimation can be effectively reduced, thus significantly improving the estimation accuracy of position, velocity, and acceleration, especially performing excellently in complex environments. By using the predator-prey escape mechanism and the artificial potential field method to predict the velocity, position, and attitude (Euler angles) of the UAV, the dynamic prediction of the UAV state based on time steps is realized and corrected by the extended Kalman filter, enabling the system to adaptively adjust the path and mission planning in a changing environment. By combining the Kalman filter with the INS system, the cumulative errors can be periodically corrected by external sensors. Especially during long-term flight missions, the cumulative errors are effectively controlled to ensure that the state estimation of the UAV does not deteriorate over time when flying in complex environments. The vector formation cooperative control of the UAV based on the INS system and the Kalman filter, combined with the predator-prey escape mechanism and the artificial potential field method, demonstrates significant beneficial effects in improving state estimation accuracy, autonomous navigation ability, dynamic path planning, and flight safety.

[0147] Furthermore, the corrected UAV inputs the state information into the UAV and performs continuous correction according to the UAV state information at the next moment, including

[0148] Applying the corrected state to the actual UAV navigation, according to the corrected state vector, generating the corresponding UAV control input data through the PID controller;

[0149] Transmitting the UAV control input data to the actuator of the UAV to control the UAV;

[0150] After the UAV executes the UAV control input data, the updated state data forms the state vector at the next moment, and is corrected again through the INS system and the Kalman filter to form a closed-loop correction.

[0151] The corrected state vector can be input into the path planning algorithm to dynamically adjust the flight route. In a multi-UAV system, the corrected state is not only used for the control of a single UAV, but also for the cooperative control between multiple UAVs. According to the corrected state of each UAV, the system can optimize formation flight or mission allocation. By sharing the corrected state of each UAV, the system can real-time adjust the relative position, velocity, and mission allocation between UAVs.

[0152] S3, securely transmits drone flight data and authenticates the drone fleet;

[0153] Preferably, secure transmission of drone flight data and authentication of drone fleets, including,

[0154] For drones that have completed their flight, the AES encryption algorithm is used to securely encrypt the drone’s flight data, and the receiving data uses the same key to decrypt the encrypted data for secure transmission;

[0155] The drone formation uses the ECC asymmetric encryption algorithm to exchange keys and authenticate the identities of the drones in the formation.

[0156] By using AES encryption during the transmission of drone flight data, it can be ensured that the data will not be eavesdropped or tampered with during the transmission process, and that sensitive information will not be obtained by unauthorized third parties. At the same time, the integrity of data transmission is ensured. Key exchange and identity authentication are performed through the ECC asymmetric encryption algorithm to ensure that each drone joining the formation is a real and trusted device. It can not only prevent malicious devices from mixing into the formation, but also ensure that data exchange between drones in the formation is safe and correct. By combining the ECC asymmetric encryption algorithm with the AES symmetric encryption algorithm, the system realizes end-to-end data encryption and identity authentication, which not only ensures the security of key exchange within the drone formation, but also uses AES encryption to quickly and efficiently encrypt and decrypt flight data.

[0157] Example 2, reference Figure 2 , which is the second embodiment of the present invention, and which is different from the previous embodiment, provides a system for a method for coordinated control of a vector formation of unmanned aerial vehicles, including:

[0158] The UAV formation module initializes the status of each UAV, including its position and speed, and builds the initial formation structure of the UAVs;

[0159] The speed and position update module initializes the UAV formation, dynamically adjusts the position and speed of the UAV based on the predator escape mechanism, and further optimizes and updates the path and speed of the UAV based on the artificial potential field method;

[0160] INS inertial navigation module, which obtains the real-time position information, speed, acceleration and attitude data of the drone through sensors such as accelerometers and gyroscopes;

[0161] The state correction module uses the Kalman filter to correct the drone state data obtained by the INS system;

[0162] A data transmission module encrypts and transmits data in a drone formation and authenticates the drones to ensure the security of data transmission and the authentication of drones in the formation.

[0163] If the above-mentioned functions are implemented in the form of software functional units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or a part of this technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions for causing a computer device (which can be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in various embodiments of the present invention. The aforementioned storage medium includes: USB flash drives, mobile hard disks, read-only memories (ROMs), random access memories (RAMs), magnetic disks, or optical discs, etc., which can store program codes.

[0164] The logic and / or steps represented in the flowchart or otherwise described herein, for example, can be considered as a definite sequence list of executable instructions for implementing logical functions, and can be specifically implemented in any computer-readable medium for use by an instruction execution system, apparatus, or device (such as a computer-based system, a system including a processor, or other systems that can fetch instructions from the instruction execution system, apparatus, or device and execute the instructions), or in combination with these instruction execution systems, apparatuses, or devices. For the purposes of this specification, a "computer-readable medium" can be any device that can contain, store, communicate, propagate, or transmit a program for use by or in combination with an instruction execution system, apparatus, or device.

[0165] More specific examples (non-exhaustive list) of computer-readable media include the following: electrical connection parts (electronic devices) having one or more wirings, portable computer disk cartridges (magnetic devices), random access memories (RAMs), read-only memories (ROMs), erasable programmable read-only memories (EPROMs or flash memories), optical fiber devices, and portable compact disc read-only memories (CDROMs). Additionally, the computer-readable medium can even be paper or other suitable media on which the program can be printed, because the program can be obtained electronically, for example, by optically scanning the paper or other media, followed by editing, interpretation, or other appropriate processing when necessary, and then stored in a computer memory.

[0166] It should be understood that various parts of the present invention can be implemented by hardware, software, firmware or a combination thereof. In the above embodiments, multiple steps or methods can be implemented by software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented by hardware, as in another embodiment, any one of the following techniques known in the art or a combination thereof can be used: discrete logic circuits having logic gate circuits for implementing logic functions on data signals, application specific integrated circuits having appropriate combinational logic gate circuits, programmable gate arrays (PGAs), field programmable gate arrays (FPGAs), etc.

[0167] It should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit them. Although the present invention has been described in detail with reference to the preferred embodiments, those of ordinary skill in the art should understand that the technical solutions of the present invention can be modified or equivalently replaced without departing from the spirit and scope of the technical solutions of the present invention, and they should all be covered within the scope of the claims of the present invention.

Claims

1. A UAV vector formation collaborative control method, characterized by: include, Construct a UAV formation and define a UAV set based on the predator-escape mechanism. Dynamically adjust the speed and position of the UAV according to the predator-escape mechanism. Use the artificial potential field method to optimize and update the speed and position of the UAV at the next moment. Deploy the INS inertial navigation system, cooperate with sensors to obtain the status information of the drone, use the Kalman filter to correct the status information of the drone, input the corrected drone status information into the drone, and continuously correct it according to the drone status information at the next moment; Securely transmit drone flight data and authenticate drone fleets; The method of constructing a drone formation and defining a drone set based on a predator-escape mechanism includes: Build a drone formation according to the number of drones, initialize the drone states, and define the drone states including the position of the i-th drone at time t and the speed of the ith UAV at time t ; Based on the predator-escape mechanism, the predator and escaper of the drone are defined, and the predator intensity is initialized. The predator set is , represents the set of neighboring drones of drone i, and the set of escaped drones is , represents the set of non-neighboring drones of drone i; The method of dynamically adjusting the speed and position of the drone according to the predator escape mechanism and optimizing and updating the speed and position of the drone at the next moment by using the artificial potential field method includes: The optimal distance between the drones is determined based on the communication distance of the drones, the detection range of the sensors, and the physical size of the drones. The optimal distance between the drones is divided by the actual distance between the current drones and multiplied by the current predator predation intensity to determine the adjusted predation intensity at the next moment. Based on the distance between the predator set and the escaper set and UAV i, the speed of the UAV is adjusted through the predator escape mechanism, which is expressed as: ； in represents the speed of drone i at the next moment t+1, Indicates the current speed of drone i, , and denote the positions of drones j, i, and k respectively, where drone j belongs to the predator set and drone k belongs to the escaper set. represents the predation intensity of drone i at the next time t+1, represents the set of predators, represents the set of escape machines; Based on the speed of drone i at the next moment t+1, the position of the drone at the current moment is adjusted by multiplying the time step by the speed of the drone at the next moment, and the position of the drone is updated; Based on the next moment's position of the drone, update the distance between the drone and the target position according to the drone's target position , and based on the maximum acceleration of the drone divided by Calculate the gravitational constant; The obstacle avoidance response time of the drone is obtained by dividing the maximum flight speed of the drone by the maximum deceleration speed. The safe distance between the drone and the obstacle is obtained by multiplying the obstacle avoidance response time by the maximum speed of the drone plus one-half of the maximum deceleration of the drone multiplied by the square of the obstacle avoidance response time. The repulsion constant is calculated based on the mass of the drone multiplied by the minimum obstacle avoidance acceleration of the drone when avoiding obstacles and multiplied by the square of the safe distance between obstacles; The gravitational gradient and repulsive gradient are calculated according to the artificial potential field method and are expressed as: ； ； in, represents the gravitational gradient of the position of the i-th drone at the next moment, is the gravitational constant, Indicates the target position of the drone, represents the repulsive force gradient of drone i at time t+1, represents the repulsion constant, represents the position of obstacle o, Indicates the safe distance between the drone and obstacles; The sum of the gravitational gradient and repulsive gradient of drone i at the next moment t+1 is taken as the total force, divided by the mass of the drone, and then summed with The sum is used as the final adjustment speed of UAV i at the next moment t+1, and the adjustment position of the UAV at the next moment t+1 is updated.

2. The UAV vector formation cooperative control method according to claim 1, characterized in that: The deployment of the INS inertial navigation system, in conjunction with sensors to obtain drone status information, includes: Deploy INS inertial navigation system on drones and collect real-time position, speed, acceleration and attitude status information of drones through sensors; The acceleration of the drone in three dimensions is measured using an acceleration sensor, and the gravity acceleration is subtracted based on the earth's gravity to obtain the linear acceleration data of the drone. The angular velocity of the UAV in three-dimensional space is measured by the gyroscope. The velocity of the UAV is calculated based on the time step by integrating the linear acceleration. The attitude of the UAV is represented by Euler angles, and the calculated UAV velocity is mapped to the global coordinate system.

3. The UAV vector formation cooperative control method according to claim 2, characterized in that: The use of a Kalman filter to correct the state information of the drone includes: The state vector of the drone is constructed based on the data collected by the INS inertial navigation and calculated including acceleration, velocity, angular velocity and position data; Based on the speed and position of the drone predicted by the predator escape mechanism and the artificial potential field method, the drone state is corrected, and the actual position and speed data of the drone at the same time are obtained based on the sensors included in the INS inertial navigation; The state vector is constructed based on the position, velocity and acceleration data of the drone, and the error covariance between the position, velocity and acceleration is constructed based on the state vector to form a covariance matrix ; The position, velocity, and acceleration data in the state vector are based on the time step Update to the next data and construct the state transfer matrix A; Based on the historical sensor data of the INS system, the process noise matrix Q is constructed according to the errors caused by the time-based changes in the drone state; Based on the covariance matrix , process noise matrix Q and state transfer matrix A update the covariance matrix, expressed as: ； in represents the updated covariance matrix, represents the transpose of the state transfer matrix; Based on the state vector of the drone, the observation matrix H is constructed, and based on the error range value of the sensor used, the observation noise matrix R is constructed; Use the Kalman filter EKF to balance the Kalman gain weight between the current predicted value and the observed value through the Kalman gain ; Based on the Euler angle at the time step and the angular velocity value, the drone attitude is updated, expressed as: ； in represents the Euler angle of drone i at time t, represents the Euler angle of drone i at time t+1, represents the time step, represents the angular velocity of UAV i at time t; The predicted state vector of the drone is constructed by using the Kalman filter to predict the drone speed and position based on the predator escape mechanism and the artificial potential field method, as well as the predicted Euler angle value. ; Based on the calculation of Kalman gain, the state prediction value based on the predator escape mechanism and the artificial potential field method is corrected, which is expressed as: ； in represents the corrected state vector, represents the predicted state vector, Represents the actual value of the state vector; Based on the corrected state vector, the covariance matrix is ​​updated and the state is corrected for the next time step through the Kalman filter.

4. The UAV vector formation cooperative control method according to claim 3, characterized in that: The correction-based UAV inputs the state information of the UAV and performs continuous correction according to the state information of the UAV at the next moment, including: Apply the corrected state to the actual UAV navigation, and generate the corresponding UAV control input data through the PID controller according to the corrected state vector; controlling the drone using drone control input data transmitted to the drone's actuators; After the drone executes the drone control input data, the updated state data will form the state vector of the next moment, which will be corrected again through the INS system and Kalman filter to form a closed-loop correction.

5. The UAV vector formation cooperative control method according to claim 4, characterized in that: The secure transmission of drone flight data and authentication of the drone fleet include: For drones that have completed their flight, the AES encryption algorithm is used to securely encrypt the drone’s flight data, and the receiving data uses the same key to decrypt the encrypted data for secure transmission; The drone formation uses the ECC asymmetric encryption algorithm to exchange keys and authenticate the identities of the drones in the formation.

6. A system based on the UAV vector formation cooperative control method according to any one of claims 1 to 5, characterized in that: include, The UAV formation module initializes the status of each UAV, including its position and speed, and builds the initial formation structure of the UAVs; The speed and position update module initializes the UAV formation, dynamically adjusts the position and speed of the UAV based on the predator escape mechanism, and further optimizes and updates the path and speed of the UAV based on the artificial potential field method; INS inertial navigation module, which obtains the real-time position information, speed, acceleration and attitude data of the drone through sensors such as accelerometers and gyroscopes; The state correction module uses the Kalman filter to correct the drone state data obtained by the INS system; The data transmission module encrypts the data in the drone formation and authenticates the drones to ensure the security of data transmission and the identity verification of drones in the team.

7. A computer device comprising: Memory and processor; The memory stores a computer program, characterized in that when the processor executes the computer program, the steps of the UAV vector formation collaborative control method described in any one of claims 1 to 5 are implemented.

8. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the steps of the UAV vector formation collaborative control method described in any one of claims 1 to 5 are implemented.

Images