Unmanned aerial vehicle vector formation cooperative control method and system

Abstract

The invention discloses an unmanned aerial vehicle vector formation cooperative control method and system, and relates to the technical field of unmanned aerial vehicle control, and the method comprises the steps: constructing an unmanned aerial vehicle formation, defining an unmanned aerial vehicle set based on a predation escape mechanism, and dynamically adjusting the speed and position of an unmanned aerial vehicle according to the predation escape mechanism. And introducing an artificial potential field method to optimize and update the speed and position of the unmanned aerial vehicle at the next moment. According to the method, the predator set and the escape machine set of the unmanned aerial vehicles are defined, the formation structure can be dynamically adjusted according to the mutual relation between the unmanned aerial vehicles and the surrounding adjacent unmanned aerial vehicles, multi-level optimization is achieved through combination of a predator escape mechanism and an artificial potential field method, and it is ensured that the unmanned aerial vehicles can be kept stable in local formation and can also be kept stable in local formation. The predation intensity mechanism ensures the reasonable position of the unmanned aerial vehicle in the formation through relative distance adjustment, and the accumulated error can be periodically corrected by using an external sensor through combined use of a Kalman filter and an INS (Inertial Navigation System).

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 drone flight data and authenticate drone fleets.

[0010] As a preferred solution of the UAV vector formation cooperative control method of the present invention, wherein: the construction of the UAV formation and the definition of the UAV set based on the predator-escape mechanism include:

[0011] 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 ;

[0012] Based on the predator-escape mechanism, the predator and escaper of the drone are defined, and the predation 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.

[0013] As a preferred solution of the UAV vector formation cooperative control method of the present invention, the method of dynamically adjusting the speed and position of the UAV according to the predator escape mechanism and citing the artificial potential field method to optimize and update the speed and position of the UAV at the next moment includes:

[0014] 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.

[0015] 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:

[0016] ;

[0017] 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 i, j, 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;

[0018] 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;

[0019] 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;

[0020] 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.

[0021] 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;

[0022] The gravitational gradient and repulsive gradient are calculated according to the artificial potential field method and are expressed as:

[0023] ;

[0024] ;

[0025] 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;

[0026] 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.

[0027] As a preferred solution of the UAV vector formation cooperative control method of the present invention, the deployment of the INS inertial navigation system and the acquisition of UAV status information in cooperation with the sensor include:

[0028] Deploy INS inertial navigation system on drones and collect real-time position, speed, acceleration and attitude status information of drones through sensors;

[0029] 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.

[0030] 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.

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

[0032] 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;

[0033] 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;

[0034] 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 ;

[0035] 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;

[0036] 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;

[0037] Based on the covariance matrix , process noise matrix Q and state transfer matrix A update the covariance matrix, expressed as:

[0038] ;

[0039] in represents the updated covariance matrix, represents the transpose of the state transfer matrix;

[0040] 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;

[0041] Use the Kalman filter EKF to balance the Kalman gain weight between the current predicted value and the observed value through the Kalman gain ;

[0042] Based on the Euler angle at the time step and the angular velocity value, the drone attitude is updated, expressed as:

[0043] ;

[0044] 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;

[0045] 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. ;

[0046] 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:

[0047] ;

[0048] in 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, the covariance matrix is ​​updated and the state is corrected for the next time step through the Kalman filter.

[0050] As a preferred solution of the UAV vector formation cooperative control method of the present invention, wherein: the correction-based 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 the corresponding UAV control input data through the PID controller according to the corrected state vector;

[0052] controlling the drone using drone control input data transmitted to the drone's actuators;

[0053] 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.

[0054] As a preferred solution of the UAV vector formation cooperative control method of the present invention, wherein: the secure transmission of UAV flight data and identity authentication of the UAV formation include:

[0055] 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;

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

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

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

[0059] 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;

[0060] 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;

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

[0062] 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.

[0063] A computer device comprises: a memory and a processor; the memory stores a computer program, and the processor implements the steps of the above-mentioned UAV vector formation collaborative control method when executing the computer program.

[0064] A computer-readable storage medium stores a computer program, which, when executed by a processor, implements the steps of the above-mentioned UAV vector formation collaborative control method.

[0065] The beneficial effects of the present invention are as follows: by defining the predator set and escaper set of the UAV, the formation structure can be dynamically adjusted according to the relationship between the UAV and the surrounding neighboring UAVs; by combining the predator escape mechanism and the artificial potential field method, multi-level optimization is achieved to ensure that the UAV can maintain stability in the local formation; the predation intensity mechanism ensures the reasonable position of the UAV in the formation through relative distance adjustment; and by combining the Kalman filter with the INS system, the accumulated 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 accompanying drawings required for use in the description of the embodiments will be briefly introduced below. Obviously, the accompanying drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other accompanying drawings can be obtained based on these accompanying drawings without paying creative work.

[0067] Figure 1 The figure is a flow chart of the UAV vector formation collaborative control method.

[0068] Figure 2 This is a schematic diagram of the structure of the UAV vector formation collaborative control system. DETAILED DESCRIPTION

[0069] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the specific embodiments of the present invention are described in detail below in conjunction with the accompanying drawings.

[0070] In the following description, many specific details are set forth to facilitate a full understanding of the present invention, but the present invention may also be implemented in other ways different from those described herein, and those skilled in the art may make similar generalizations without violating the connotation of the present invention. Therefore, the present invention is not limited to the specific embodiments disclosed below.

[0071] Secondly, the term "one embodiment" or "embodiment" as used herein refers to a specific feature, structure, or characteristic that may be included in at least one implementation of the present invention. The term "in one embodiment" that appears in different places in this specification does not necessarily refer to the same embodiment, nor is it a separate or selective embodiment that is mutually exclusive with other embodiments.

[0072] Example 1, reference Figure 1 , which is the first embodiment of the present invention, and provides a UAV vector formation cooperative control method, the UAV vector formation cooperative control method includes:

[0073] S1, build 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, and use the artificial potential field method to optimize and update the speed and position of the UAV at the next moment;

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

[0075] 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 ;

[0076] Based on the predator-escape mechanism, the predator and escaper of the drone are defined, and the predation 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.

[0077] By defining the predator set and escaper set of the drone, the formation structure can be dynamically adjusted according to the relationship between the drone and the surrounding neighboring drones, ensuring the flexibility and dynamic adjustment ability of the drone formation, so that the drone can make real-time adjustments according to different mission requirements in a complex environment. Through the predation and escape mechanism, the formation can be adaptively adjusted according to the dynamic changes of the external environment and the mission to maintain a flexible formation form. By dividing the drones into predators and escapers, the system can make adaptive adjustments according to the relative distance of the drones. When the distance between the drones is too close, the escape mechanism enables the drone to avoid other drones that are too close in time to prevent collision. When the distance is moderate, the predation mechanism makes the drone tend to its neighbors to maintain a reasonable formation structure. Through the initialization setting of the predation intensity, the drone can automatically form a formation according to the distance and the predation and escape mechanism, and dynamically adjust the formation form.

[0078] Furthermore, the speed and position of the drone are dynamically adjusted according to the predator escape mechanism, and the artificial potential field method is used to optimize and update the speed and position of the drone at the next moment, including:

[0079] The optimal distance between 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 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. The predation intensity is dynamically adjusted according to the distribution of drones, expressed as:

[0080] ;

[0081] ;

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

[0083] 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:

[0084] ;

[0085] 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 i, j, 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;

[0086] In the formula, the calculation of the first term is is the predation behavior of drone i and drone j, indicating the direction and magnitude of drone i being attracted by neighboring drones. The closer the neighboring drone is, the greater the influence it has. The calculation of the second term is is the escape behavior of UAV i and UAV k, indicating that UAV i avoids the direction and amplitude of the escape aircraft according to the location of the escape aircraft set, and the farther the distance, the smaller the impact;

[0087] 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, which is expressed as:

[0088] ;

[0089] in represents the position of drone i at the next moment t+1, Represents the position of the drone at the current time t, is the time step, which indicates the length of each time step in continuous time, in seconds (s);

[0090] 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;

[0091] 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.

[0092] In the process of calculating the safe distance between the drone and the obstacle, the distance traveled within the reaction time is calculated based on the obstacle avoidance response time multiplied by the maximum speed of the drone, and the calculation content of half of the maximum deceleration of the drone multiplied by the square of the obstacle avoidance response time represents the additional distance traveled by the drone during the deceleration process; the sum of the two parts constitutes the minimum safe distance of the drone to ensure that there is enough time and distance for obstacle avoidance operations after sensing the obstacle;

[0093] 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;

[0094] The gravitational constant is calculated by dividing the maximum speed of the drone by the distance between the drone and the target. When the drone is far away from the target point, the formula calculates a smaller gravitational constant, and when the drone approaches the target point, the gravitational constant becomes larger, which conforms to the basic principle of physics that the gravitational field changes with distance, and ensures that the drone accelerates or decelerates according to the distance from the target, making the drone's path planning more dynamically adaptable.

[0095] The repulsive force constant calculated based on the safe distance between the drone and the obstacle, the minimum obstacle avoidance acceleration and the mass of the drone conforms to the classical mechanics formula ( ), and ensure that the drone can effectively generate repulsive force in different environments regardless of the distance of obstacles, so that the use of repulsive force constants and the substitution of them into the calculation of repulsive force gradients can effectively maintain the safe distance position of the drone. The calculation of the safe distance between the drone and obstacles combines the speed, response time and deceleration of the drone, and dynamically considers the current speed and reaction time of the drone, ensuring that even in high-speed flight, the drone can avoid obstacles in time and maintain a safe distance, ensuring the robustness and safety of the drone system;

[0096] The gravitational gradient and repulsive gradient are calculated according to the artificial potential field method and are expressed as:

[0097] ;

[0098] ;

[0099] in, represents the gravitational gradient of the next moment position of the i-th drone (indicates the gravitational gradient of the drone from the next moment position to direction and magnitude of movement), is the gravitational constant, represents the target position of the drone, represents the repulsive force gradient of drone i at time t+1, which indicates how the drone moves according to the distance from obstacles or other drones to avoid collision. represents the repulsion constant, represents the position of obstacle o, Indicates the safe distance between the drone and obstacles;

[0100] 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.

[0101] The gravity gradient is calculated based on the vector difference between the target point and the current position of the drone, so that the gravity increases with the distance to the target. When the drone is far away from the target, the larger gravity pushes the drone to accelerate towards the target, while when the drone approaches the target, the smaller gravity ensures that the drone gradually decelerates to avoid overshoot, ensuring the flight dynamics of the drone at different distances;

[0102] The repulsion gradient formula is based on the inverse square relationship of the distance, which is similar to the Coulomb force or gravity formula in physics. It ensures that when the drone approaches an obstacle, the repulsion will increase sharply, forcing the drone to quickly move away from the obstacle. When the distance to the obstacle is far, the repulsion will decrease rapidly until it is zero, ensuring the locality and efficiency of the repulsion. By setting a safe distance, it ensures that the drone only generates repulsion when approaching an obstacle, and is not affected by the repulsion when far away from the obstacle.

[0103] By calculating the total force, the drone can benefit from the guiding effect of gravity and the obstacle avoidance function of repulsion at the same time. Gravity prompts the drone to move towards the target point, while repulsion ensures that the drone will not collide with obstacles, ensuring that the drone's path optimization is not only moving towards the target point, but also adapting to the obstacle avoidance needs in a dynamic environment, making the drone more flexible in complex environments;

[0104] The drone formation can adaptively adjust the predation intensity according to the initial position, optimal distance and actual distance of each drone, and dynamically update the speed and position to achieve mutual coordination and formation maintenance between drones. The adaptive adjustment based on the predation 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 status in the formation.

[0105] The adaptive adjustment formula of predation intensity enables the UAV to dynamically adjust its 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 use environment of UAVs. By combining predation intensity with optimal distance, the UAV can automatically adjust its relative position under different mission requirements, thereby dynamically optimizing the path. At the same time, through the escape mechanism, the UAV can avoid overly dense neighboring UAVs and obstacles to prevent collisions. This dynamic adjustment maintains the flexibility of the formation and can also adapt to dynamic external environments, such as sudden obstacles and sudden changes in mission requirements.

[0106] By introducing the artificial potential field method, the drone is further guided toward the target point through gravity, and the drone is ensured to avoid obstacles or other drones and avoid collisions through repulsion. This mechanism ensures that the drone can autonomously avoid obstacles in complex environments while maintaining an effective path to the target.

[0107] At the same time, through the combination of the predation escape mechanism and the artificial potential field method, multi-level optimization is achieved to ensure that the UAV can maintain stability in the local formation and achieve global path optimization and obstacle avoidance capabilities. The predation intensity mechanism ensures the reasonable position of the UAV in the formation by adjusting the relative distance. The artificial potential field method further adjusts the flight direction and speed of the UAV, optimizes its path toward the target point, and ensures that collisions are avoided in complex environments, so that the UAVs can adjust their relative positions while achieving global path optimization and obstacle avoidance through the artificial potential field method, so that the UAV vector formation can fly along the optimal path during flight without relying on external intervention, achieving efficient and stable path planning.

[0108] S2, deploy the INS inertial navigation system, cooperate with the sensor to obtain the drone status information, use the Kalman filter to correct the drone status information, input the corrected drone status information into the drone, and continuously correct it according to the drone status information at the next moment;

[0109] Preferably, an INS inertial navigation system is deployed to cooperate with sensors to obtain drone status information, including:

[0110] Deploy INS inertial navigation system on drones and collect real-time position, speed, acceleration and attitude status information of drones through sensors;

[0111] 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.

[0112] 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.

[0113] By performing gravity compensation on the measurement results of the acceleration sensor, the true 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. 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 grasp its accurate position when performing tasks. By using the INS system, the UAV can still navigate through its own inertial sensor measurements without relying on external signals. By performing gravity compensation on the acceleration sensor and angular velocity measurement 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 Euler angles to map the speed of the UAV to the global coordinate system, the UAV can better adapt to the navigation needs of complex three-dimensional space, 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 space.

[0114] Furthermore, the Kalman filter is used to correct the state information of the UAV, including:

[0115] 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;

[0116] 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;

[0117] 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 , expressed as:

[0118] ;

[0119] in , and represent the variance of position, velocity and acceleration respectively, , , , , and Represent the covariance terms of position, velocity and acceleration respectively;

[0120] 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, which is expressed as:

[0121] ;

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

[0123] Based on the historical sensor data of the INS system, the process noise matrix Q is constructed according to the error caused by the change of the drone state over time, which is expressed as:

[0124] ;

[0125] in , and Respectively represent the state change error noise of position, velocity, and acceleration;

[0126] Based on the covariance matrix , process noise matrix Q and state transfer matrix A update the covariance matrix, expressed as:

[0127] ;

[0128] in represents the updated covariance matrix, represents the transpose of the state transfer matrix;

[0129] Based on the state vector of the drone, the observation matrix H is constructed and expressed as:

[0130] ;

[0131] Based on the error range value of the sensor used, the observation noise matrix R is constructed and expressed as:

[0132] ;

[0133] in , and Respectively represent the detection error noise of position, velocity, 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] in 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 angle at the time step and the angular velocity value, the drone attitude is updated, expressed as:

[0138] ;

[0139] 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;

[0140] 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. , expressed as:

[0141] ;

[0142] 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:

[0143] ;

[0144] in 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, the covariance matrix is ​​updated and the state is corrected for the next time step through the Kalman filter.

[0146] The INS system collects the real-time position, speed, acceleration and angular velocity data of the UAV, and based on the state prediction and correction mechanism of the Kalman filter, the system can significantly improve the state estimation accuracy of the UAV. The errors that may be accumulated in the INS system are periodically corrected through external sensor data to ensure that the accuracy of state estimation remains at a high level during long-term flight. The dynamic update of the covariance matrix and the combined use of the observation noise matrix and the process noise matrix can effectively reduce the interference of noise on the state estimation, thereby significantly improving the estimation accuracy of position, speed and acceleration, especially in complex environments. The predator escape mechanism and the artificial potential field method are used to predict the speed, position and attitude of the UAV (Euler angle ), dynamic prediction of UAV state based on time step is realized, and correction is made through extended Kalman filter, so that the system can adaptively adjust the path and mission planning in a changing environment. By combining the Kalman filter with the INS system, external sensors can be used to periodically correct the accumulated error. Especially in long-term flight missions, the accumulated error is effectively controlled to ensure that the state estimation of the UAV will not deteriorate over time when flying in a complex environment. The UAV vector formation collaborative control based on INS system and Kalman filter, combined with the predator escape mechanism and artificial potential field method, has shown significant beneficial effects in improving state estimation accuracy, autonomous navigation capability, dynamic path planning and flight safety.

[0147] Further, based on the corrected drone state information input into the drone, continuous correction is performed according to the drone state information at the next moment, including,

[0148] 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;

[0149] controlling the drone using drone control input data transmitted to the drone's actuators;

[0150] 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.

[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 coordinated control between multiple UAVs. According to the corrected state of each UAV, the system can optimize formation flight or task allocation. By sharing the corrected state of each UAV, the system can adjust the relative position, speed and task allocation between UAVs in real time.

[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] 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 authentication of drones in the team.

[0163] If the 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, or the part that contributes to the prior art, or the part of the technical solution, can be embodied in the form of a software product. The computer software product is stored in a storage medium, including several instructions to enable a computer device (which can be a personal computer, server, or network device, etc.) to perform all or part of the steps of the methods described in various embodiments of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk, 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 an ordered list of executable instructions for implementing logical functions, and can be embodied in any computer-readable medium for use by an instruction execution system, device or apparatus (such as a computer-based system, a system including a processor, or other system that can fetch instructions from an instruction execution system, device or apparatus and execute instructions), or in conjunction with such instruction execution systems, devices or apparatuses. For the purposes of this specification, "computer-readable medium" can be any device that can contain, store, communicate, propagate or transmit a program for use by an instruction execution system, device or apparatus, or in conjunction with such instruction execution systems, devices or apparatuses.

[0165] More specific examples of computer-readable media (a non-exhaustive list) include the following: an electrical connection with one or more wires (electronic device), a portable computer disk case (magnetic device), a random access memory (RAM), a read-only memory (ROM), an erasable and programmable read-only memory (EPROM or flash memory), an optical fiber device, and a portable compact disk read-only memory (CDROM). In addition, the computer-readable medium may even be a paper or other suitable medium on which the program is printed, since the program may be obtained electronically, for example, by optically scanning the paper or other medium, followed by editing, deciphering or, if necessary, processing in another suitable manner, and then stored in a computer memory.

[0166] It should be understood that the 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, it can be implemented by any one of the following technologies known in the art or their combination: a discrete logic circuit with a logic gate circuit for implementing a logic function for a data signal, a dedicated integrated circuit with a suitable combination of logic gate circuits, a programmable gate array (PGA), a field programmable gate array (FPGA), etc.

[0167] It should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solutions of the present invention may be modified or replaced by equivalents without departing from the spirit and scope of the technical solutions of the present invention, which should all be included in 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.

2. The UAV vector formation cooperative control method according to claim 1, characterized in that: The construction of drone formation and definition of drone set based on predator-escape mechanism, include, 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 predation 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.

3. The UAV vector formation cooperative control method according to claim 2, characterized in that: 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 i, j, 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, represents 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.

4. The UAV vector formation cooperative control method according to claim 3, 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 by the 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.

5. The UAV vector formation cooperative control method according to claim 4, 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.

6. The UAV vector formation cooperative control method according to claim 5, 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.

7. The UAV vector formation cooperative control method according to claim 6, 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.

8. A system based on the UAV vector formation cooperative control method according to any one of claims 1 to 7, 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.

9. 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 7 are implemented.

10. 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 drone vector formation collaborative control method described in any one of claims 1 to 7 are implemented.

Images