Unmanned aerial vehicle cluster trajectory planning method, formation control method, equipment and medium

Abstract

The invention relates to the technical field of unmanned aerial vehicle clusters, in particular to an unmanned aerial vehicle cluster trajectory planning method, a formation control method, equipment and a medium. The method comprises the following steps: firstly, pre-planning a spline curve required by an unmanned aerial vehicle cluster to reach a total target point, then selecting nodes on the spline curve to construct a point set, then constructing four cost functions corresponding to the point set, and optimizing the point set according to the four cost functions, namely, optimizing the position of each node in the point set, so as to obtain an optimized target point; and planning an actual flight path different from the spline curve again by using each node in the optimized point set. As two of the four cost functions consider the dynamic and static obstacles, each node in the point set optimized based on the four cost functions can avoid the positions of the two obstacles as much as possible; therefore, the actual flight path constructed based on the optimized point set can reduce the potential risk that the unmanned aerial vehicle cluster collides with the obstacle.

Description

TECHNICAL FIELD

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

[0002] Each unmanned aerial vehicle constituting an unmanned aerial vehicle cluster cooperates to complete a designated task. Before the unmanned aerial vehicle flies to a destination to perform a task, a trajectory of the unmanned aerial vehicle cluster needs to be planned so that the unmanned aerial vehicle cluster flies along the trajectory to the destination. In the prior art, a trajectory is planned only by selecting points on a map, without considering obstacles that may be encountered during flight, resulting in the unmanned aerial vehicle cluster colliding with obstacles when flying along the planned trajectory.

[0003] In summary, the unmanned aerial vehicle cluster flying along the trajectory planned by the prior art increases the risk of collision with obstacles.

[0004] Therefore, the prior art still needs to be improved and enhanced. SUMMARY

[0005] To solve the above technical problems, the present application provides an unmanned aerial vehicle cluster trajectory planning method, formation control method, device and medium, which solves the problem that the unmanned aerial vehicle cluster flying along the trajectory planned by the prior art increases the risk of collision with obstacles.

[0006] To achieve the above purpose, the present application adopts the following technical solutions: In a first aspect, the present application provides an unmanned aerial vehicle cluster trajectory planning method, comprising: obtaining a general target point to be reached by an unmanned aerial vehicle cluster, and obtaining a spline curve based on the general target point, the spline curve being used to represent a pre-planned trajectory required for the unmanned aerial vehicle cluster to fly to the general target point; constructing a point set, the nodes in the point set being points selected from the spline curve; determining first cost data corresponding to the point set, the first cost data being used to represent the smoothness of the unmanned aerial vehicle cluster flying along each node in the point set; determining second cost data corresponding to the point set, the second cost data being used to represent the dynamic feasibility data of the unmanned aerial vehicle cluster flying along each node in the point set; obtaining the position of a static obstacle, and determining third cost data corresponding to the point set according to the position of the static obstacle and the position of the node, the third cost data being used to represent the risk degree of the unmanned aerial vehicle cluster colliding with the static obstacle when flying along each node in the point set; acquire a position and a speed of a dynamic obstacle, determine fourth cost data corresponding to the point set according to the position and the speed of the dynamic obstacle and the position of the node, the fourth cost data being used to represent a risk degree of the UAV cluster colliding with the dynamic obstacle when flying along each node in the point set; optimize the point set according to the first cost data, the second cost data, the third cost data and the fourth cost data corresponding to the point set, and construct an actual flight trajectory of the UAV cluster through the point set after optimization.

[0007] In an implementation manner, the first cost data corresponding to the point set is determined by: acquiring node positions included in the point set and acquiring a flight time length of the UAV cluster between nodes preset in advance; determining an acceleration of the UAV cluster at the nodes included in the point set according to each two node positions and the flight time length; determining the first cost data corresponding to the point set according to the acceleration.

[0008] In an implementation manner, the second cost data corresponding to the point set is determined by: acquiring a maximum flight speed and a maximum flight acceleration of the UAV cluster preset in advance; determining a speed and an acceleration of the UAV cluster at the nodes included in the point set; determining the second cost data corresponding to the point set according to the maximum flight speed and the maximum flight acceleration and the speed and the acceleration of the node.

[0009] In an implementation manner, the third cost data corresponding to the point set is determined according to the position of the static obstacle and the position of the node, comprising: acquiring a safety distance preset in advance between the nodes included in the point set and the static obstacle; obtaining an actual distance between the static obstacle and the node according to the position of the static obstacle and the position of the node; determining the third cost data corresponding to the point set according to the safety distance and the actual distance between the static obstacle and the node.

[0010] In an implementation manner, the fourth cost data corresponding to the point set is determined according to the position and the speed of the dynamic obstacle and the position of the node, comprising: determining a lateral speed and a longitudinal speed in the speed of the dynamic obstacle; According to the position of the dynamic obstacle, the lateral and longitudinal velocities, and the positions of the nodes included in the point set, fourth cost data corresponding to the point set is determined.

[0011] In an implementation manner, the point set is optimized according to the first cost data, the second cost data, the third cost data, and the fourth cost data corresponding to the point set, and an actual flight trajectory of the UAV cluster is constructed by using the point set after optimization, including: The first cost data, the second cost data, the third cost data, and the fourth cost data corresponding to the point set are weighted to obtain total cost data of the point set; The point set is optimized until the total cost data corresponding to the point set after optimization is minimum; The position of the point included in the point set corresponding to the minimum total cost data is taken as the position of a control point, and the position of the control point is the position to be worked on by the UAV cluster; According to the position of the control point, the actual flight trajectory of the UAV cluster is constructed.

[0012] In a second aspect, an embodiment of the present application further provides a formation control method, including: An actual flight trajectory of a UAV cluster is obtained, the actual flight trajectory is a trajectory obtained by a leader based on the UAV cluster trajectory planning method described above, and the UAV cluster is composed of the leader and a follower; A real-time position of the leader in a current time period when flying along the actual flight trajectory is collected; According to the yaw angle and the real-time position of the leader and a preset position of the follower in the formation, a target position to be reached by the follower in the current time period is determined; An actual position of the follower in the current time period is collected, and an input of a controller of the follower is determined according to the actual position of the follower and the target position of the follower, the controller being used to keep the formation of the follower based on the input.

[0013] In an implementation manner, the follower and the leader are both quadcopters.

[0014] In a third aspect, an embodiment of the present application further provides a terminal device, including a memory, a processor, and a UAV cluster trajectory planning program stored in the memory and executable on the processor, and when the processor executes the UAV cluster trajectory planning program, the steps of the UAV cluster trajectory planning method described above are implemented. Alternatively, the terminal device comprises a memory, a processor, and a formation control program stored in the memory and executable on the processor, and the processor implements the steps of the formation control method when executing the formation control program.

[0015] In a fourth aspect, the embodiments of the present application further provide a computer readable storage medium, wherein the computer readable storage medium stores a UAV cluster trajectory planning program, and the processor implements the steps of the UAV cluster trajectory planning method when executing the UAV cluster trajectory planning program. Alternatively, the computer readable storage medium stores a formation control program, and the processor implements the steps of the formation control method when executing the formation control program.

[0016] Beneficial effects: The present application firstly pre-plans a spline curve required by the UAV cluster to reach the overall target point, then selects nodes on the spline curve to construct a point set, and then constructs four cost functions corresponding to the point set, optimizes the point set according to the four cost functions, that is, optimizes the positions of the nodes in the point set, and plans an actual flight trajectory different from the spline curve again by using the nodes in the point set after optimization. Since two of the four cost functions in the present application consider static obstacles and dynamic obstacles, the nodes in the point set after optimization based on the four cost functions can avoid the positions of the two kinds of obstacles as much as possible, and therefore the actual flight trajectory constructed based on the point set after optimization can reduce the potential risk of the UAV cluster colliding with the obstacles. BRIEF DESCRIPTION OF DRAWINGS

[0017] Figure 1 The figure is a whole flowchart of the present application; Figure 2 The figure is a schematic diagram of the fourth cost data in the embodiments of the present application; Figure 3 The figure is an internal structure principle block diagram of the terminal device provided by the embodiments of the present application. DETAILED DESCRIPTION

[0018] The technical solutions in the present application are described clearly and completely in combination with the embodiments and the accompanying drawings of the specification. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor fall within the scope of protection of the present application.

[0019] It is found through research that each unmanned aerial vehicle in the unmanned aerial vehicle cluster cooperates to complete the designated task, and before the unmanned aerial vehicle flies to the destination to perform the task, the trajectory of the unmanned aerial vehicle cluster needs to be planned so that the unmanned aerial vehicle cluster flies along the trajectory to the destination. When planning the trajectory of the unmanned aerial vehicle cluster, the prior art only selects points on a map to plan the trajectory without considering obstacles that may be encountered during flight, resulting in that the unmanned aerial vehicle cluster flying along the planned trajectory may collide with obstacles.

[0020] To solve the above technical problems, the present application provides a method for planning a trajectory of an unmanned aerial vehicle cluster, a formation control method, equipment and a medium, which solve the problem that the unmanned aerial vehicle cluster flying along the trajectory planned by the prior art increases the risk of collision with obstacles.

[0021] For example, the unmanned aerial vehicle cluster composed of a plurality of followers and a leader needs to fly from the current location A to the destination B. The coordinates of the overall target point of B are sent to the leader, the leader plans a spline curve according to the overall target point, then selects ten nodes on the spline curve according to a fixed time interval to form a first point set, and ensures that the selected nodes can cover the spline curve, and then calculates the cost data corresponding to the point set of the ten nodes according to the smoothness, dynamic feasibility data and whether the ten nodes will cause the unmanned aerial vehicle cluster to collide with dynamic and static obstacles.

[0022] The position of each node in the point set is optimized by using the adjacent nodes of the node, the cost data corresponding to the point set after each optimization is calculated, until the minimum cost data is obtained, and the point set corresponding to the minimum cost data is used as the final point set, the positions of the ten points in the final point set are used to construct a trajectory different from the spline curve, and the trajectory is used as the actual flight trajectory of the unmanned aerial vehicle cluster. The adjacent nodes of each node are points located in the adjacent space of the spline curve, and the adjacent nodes are not on the original spline curve.

[0023] Embodiment one provides a method for planning a trajectory of an unmanned aerial vehicle cluster. The method for planning a trajectory of an unmanned aerial vehicle cluster can be applied to a terminal device, which can be a terminal product with a data processing function, such as a computer on an unmanned aerial vehicle. Figure 1 The method for planning a trajectory of an unmanned aerial vehicle cluster specifically includes the following steps: S100, obtaining an overall target point to be reached by an unmanned aerial vehicle cluster, and obtaining a spline curve based on the overall target point, the spline curve being used to represent a pre-planned trajectory required for the unmanned aerial vehicle cluster to fly to the overall target point; S200, constructing a point set, the nodes in the point set being points selected from the spline curve; S300, determine first cost data corresponding to the point set, the first cost data being used to represent smoothness of the UAV cluster flying along each node in the point set; S400, determine second cost data corresponding to the point set, the second cost data being used to represent dynamic feasibility data of the UAV cluster flying along each node in the point set; S500, obtain a position of a static obstacle, and determine third cost data corresponding to the point set according to the position of the static obstacle and the position of the node, the third cost data being used to represent a risk degree of the UAV cluster colliding with the static obstacle when flying along each node in the point set; S600, obtain a position and a speed of a dynamic obstacle, and determine fourth cost data corresponding to the point set according to the position and the speed of the dynamic obstacle and the position of the node, the fourth cost data being used to represent a risk degree of the UAV cluster colliding with the dynamic obstacle when flying along each node in the point set; S700, optimize the point set according to the first cost data, the second cost data, the third cost data and the fourth cost data corresponding to the point set, and construct an actual flight trajectory of the UAV cluster through the point set after optimization.

[0024] In the embodiment, the UAV cluster is composed of one UAV as a leader and several UAVs as followers. The leader UAV is registered as a master in the communication network, and the other UAVs are registered as slaves in the communication network.

[0025] All onboard computers of the UAVs are connected to the same local area network, so that the UAVs in the cluster can communicate with each other. The ROS environment is built in the Ubuntu system of each onboard computer. Ubuntu represents an open source computer operating system, and ROS represents Robot Operating System.

[0026] The UAV cluster keeps the formation unchanged during flight, that is, the relative positions between the UAVs remain unchanged.

[0027] In the embodiment, the step S100 of obtaining the spline curve according to the overall target point is prior art, and the spline curve is a B-spline curve. B represents Basis, and the B-spline curve is a group of curves composed of basis functions and node linear combinations.

[0028] In this embodiment, the specific steps of constructing the point set in step S200 include: randomly selecting nodes from the spline curve to construct the point set, or selecting nodes that are far away from the static obstacle from the spline curve to construct the point set.

[0029] In this embodiment, determining the first cost data corresponding to the point set in step S300 includes the following specific steps: obtaining the positions of two nodes contained in the point set, and obtaining the pre-set flight time of the UAV cluster between the two nodes; determining the acceleration of the UAV cluster at the nodes contained in the point set based on the positions of the two nodes and the flight time; and determining the first cost data corresponding to the point set based on the acceleration.

[0030] That is to use The first cost data is calculated using the following three formulas. : (1) (2) (3) The locations of the two nodes mentioned above include and ,in The first in the set of representative points The position of each node. The first in the set of representative points The position of each node. This represents the pre-defined flight time of the drone swarm between two nodes. In order to ensure that the drone swarm can fly from the starting point to the overall target point (i.e., the destination) within the specified total time, the flight time between the two nodes needs to be specified in advance. Representing the drone swarm in the The velocity at each node position Representing the drone swarm in the The velocity at each node location. Representing the drone swarm in the Acceleration at each node location. This represents the number of nodes in the set of points. represent The square of the L2 norm.

[0031] Because it is based on acceleration calculation The greater the acceleration, the less smooth the flight trajectory; the smaller the acceleration, the smoother the flight trajectory. It affected the smoothness of the flight trajectory.

[0032] The optimization of the point set based on the first cost data can reduce flight energy consumption and improve flight safety.

[0033] In the embodiment, the determination of the second cost data corresponding to the point set in step S400 comprises the following specific steps: obtaining the maximum flight speed and the maximum flight acceleration of the UAV cluster preset; determining the speed and acceleration of the UAV cluster at the nodes contained in the point set; and determining the second cost data corresponding to the point set according to the maximum flight speed and the maximum flight acceleration and the speed and acceleration of the nodes.

[0034] That is, the second cost data is represented by C2 and is calculated by the following formula: ; (4) In the formula, Vmax represents the maximum flight speed allowed by the UAV cluster, Vmax represents the maximum flight speed allowed by the UAV cluster, Vmax represents the maximum flight speed allowed by the UAV cluster, Vmax represents the maximum flight acceleration allowed by the UAV cluster, Vmax represents the maximum flight acceleration allowed by the UAV cluster, Vmax represents the maximum flight acceleration allowed by the UAV cluster, Vmax represents the maximum flight acceleration allowed by the UAV cluster, Vmax represents the maximum flight acceleration allowed by the UAV cluster, Vmax represents the maximum flight acceleration allowed by the UAV cluster, Vmax represents the maximum flight acceleration allowed by the UAV cluster, Vmax represents the maximum flight acceleration allowed by the UAV cluster. Vmax represents the maximum flight acceleration allowed by the UAV cluster. Vmax represents the maximum flight acceleration allowed by the UAV cluster. Vmax represents the maximum flight acceleration allowed by the UAV cluster.

[0035] In the embodiment, the parts in which the speed and acceleration of each node in the point set exceed the maximum allowed value are accumulated, and the accumulation result represents the dynamic feasibility of the UAV cluster flying along each node in the point set.

[0036] In the embodiment, the determination of the third cost data corresponding to the point set in step S500 comprises the following specific steps: obtaining a safety distance preset between the nodes contained in the point set and the static obstacle; obtaining an actual distance between the static obstacle and the nodes according to the position of the static obstacle and the position of the nodes; and determining the third cost data corresponding to the point set according to the safety distance and the actual distance between the static obstacle and the nodes.

[0037] That is, the third cost data is represented by C3 and is calculated by the following formula: ​​​​ (5) In the formula, The first in the set of representative points The position of each node Actual distance to static obstacles, subscript This represents a static obstacle. In this embodiment, the safe distance is the minimum safe distance between the node and the static obstacle. Represents the minimum safe distance, where It represents the smallest. represent The minimum of 0 and 0.

[0038] In this embodiment, static obstacles include tall buildings and fixed-position aerial work machinery.

[0039] In this embodiment, step S600, which determines the fourth cost data corresponding to the point set based on the position and velocity of the dynamic obstacle and the position of the node, includes the following specific steps: determining the lateral velocity and longitudinal velocity of the dynamic obstacle; and determining the fourth cost data corresponding to the point set based on the position of the dynamic obstacle, the lateral velocity and longitudinal velocity, and the position of the node contained in the point set.

[0040] That is to use The fourth cost data is calculated using the following formula. : (6) In the formula, The parameter representing the rate of change in the drone swarm. The base of the natural logarithm. The first in the set of representative points The distance between each node and the dynamic obstacle along the horizontal axis of the world coordinate system. The first in the set of representative points The distance between each node and the dynamic obstacle along the vertical axis of the world coordinate system. and The corresponding times are all before the drone swarm takes off, that is, before the drone swarm takes off, the distance between the dynamic obstacle and the node at the current time is calculated.

[0041] In the formula, Representing dynamic obstacles (using) (Representing the lateral velocity of a dynamic obstacle) along the horizontal axis of the world coordinate system. and These represent two different functions that determine the change in velocity. the direction of is the direction away from the node or the value of is 0 and the value of is 0 and the value of is the absolute value of ; when the direction of is the direction towards the node, then the value of is the absolute value of and the value of is 0.

[0042] represents the lateral velocity of the dynamic obstacle in the longitudinal direction of the world coordinate system, when the direction of is the direction away from the node or the value of is 0 and the value of is 0 and the value of is the absolute value of, when the direction of is the direction towards the node, then the value of is the absolute value of and the value of is 0.

[0043] represents the coordinate of in the longitudinal axis (denoted by ), and represents the coordinate of the dynamic obstacle in the longitudinal axis, and represents the coordinate position comparison function, when is less than or equal to , then the value is 1; when is greater than , then the value is 0.

[0044] represents the coordinate of in the longitudinal axis (denoted by ), and represents the coordinate of the dynamic obstacle in the longitudinal axis, when is less than or equal to , then the value is 1; when is greater than , then the value is 0.

[0045] represents the coordinate position size judgment function, when is greater than or equal to , then the value is 1; when is less than , then is 0. When is greater than or equal to , then is 1; when is less than , then is 0.

[0046] The dynamic obstacle in this embodiment includes an aerial vehicle with a slow moving speed.

[0047] The function image between the fourth cost data of this embodiment and the distance is shown in FIG. 4, and the cost value in Figure 2 is Figure 2 . .

[0048] In this embodiment, step S700 includes the following specific steps: performing weighted calculation on the first cost data, the second cost data, the third cost data and the fourth cost data corresponding to the point set to obtain total cost data of the point set; optimizing the point set until the total cost data corresponding to the point set after optimization is minimum; taking the position of the point included in the point set corresponding to the minimum total cost data as the position of the control point, the position of the control point being the position to be worked by the UAV cluster; and constructing the actual flight trajectory of the UAV cluster according to the position of the control point.

[0049] The total cost data of the point set is , wherein , , and are weights of , , and , respectively.

[0050] Each point set is based on steps S300, S400, S500, S600, S700 to calculate the total cost data corresponding to the point set, compare the total cost data corresponding to each point set, take the position of the node included in the point set corresponding to the minimum total cost data as the position of the control point, and plan the actual flight trajectory of the UAV cluster again based on the position of the control point, that is, replace the original point set with the optimal point set obtained from the adjacent space of the original spline curve, so as to construct a new spline curve to optimize the point set, so that the control points included in the point set after optimization can form a trajectory different from the spline curve, and the trajectory is used as the actual flight trajectory.

[0051] Example 2, based on Example 1, provides a formation control method, including: acquiring the real-time position of the navigator flying along the actual flight trajectory during the current time period; determining the target position that the follower needs to reach during the current time period based on the navigator's yaw angle and real-time position, as well as the preset position of the follower in the formation; acquiring the actual position of the follower during the current time period, and determining the controller input of the follower based on the actual position of the follower and the target position of the follower, wherein the controller is used to enable the follower to maintain the formation based on the input.

[0052] use This represents the target location that the follower needs to reach at the current moment, where , and Representing the first One follower (using) The horizontal, vertical, and vertical coordinates that the follower needs to reach in the world coordinate system at the current moment.

[0053] (7) (8) (9) In the formula, , and These represent the navigator's x-coordinate, y-coordinate, and y-coordinate in the world coordinate system at the current moment. Represents the real-time location during the current time period. Representing the leader, The yaw angle represents the navigator's angle. , and Representing the first The expected distances between a follower and the navigator in the body coordinate system along the horizontal axis, the expected distance along the vertical axis, and the expected distance along the vertical axis. The body coordinate system represents the preset position of the follower in the formation, which is the same as the navigator's coordinate system.

[0054] The above gives the target position that the follower should reach in the current time period. The current time period is a certain amount of time, for example, ten minutes. Any moment within ten minutes is considered the current moment. The target position (the target position is...) By sampling, the target sampling position of the follower at the sampling time point is obtained. Representing the The first follower in the The target sampling location at each sampling time point is used actual position of the i-th follower at the j-th sampling time point, the value of the input of the controller of the i-th follower is obtained by solving equation (10).

[0055] ; (10) wherein, represents the number of sampling time points, and T represents the transpose of a matrix.

[0056] determines the size of , and thus the value of corresponding to the minimum value of equation (10) can be calculated. represents a positive definite matrix.

[0057] The embodiment is to solve equation (10) under the following constraint condition: ; represents the state of the i-th follower at the j-th sampling time point, represents the state of the i-th follower at the j-th sampling time point, represents the input of the controller of the i-th follower at the j-th sampling time point, represents and sampled at a sampling interval applies the UAV dynamics model, represents the UAV dynamics model, represents the minimum value of the measured UAV state, represents the maximum value of the measured UAV state, represents the minimum input of the controller, represents the maximum input of the controller, represents the objective state of the UAV at each time of entering the solver, the objective state of the UAV includes the measured position, velocity, attitude, angular velocity, represents the initialization condition input to the constraint condition, 0 represents the initial, the initial is .

[0058] The UAV dynamics model in the embodiment is as follows, and the UAV kinematics model is used to predict the state at the next time according to the state and input at the previous time: ​​​​​​​​​​ ; use Represents the spatial location of the drone. represent The derivative of Represents the speed of the followers. represent The derivative of The sum of thrust generated by the four rotors of the drone acting as a follower. Represents gravitational acceleration. Represents quality, represent directional vector, , Represents the attitude data of the drone as a follower. represent The derivative of The angular velocity of the drone acting as a follower. Represents the airframe torque of the drone. The inertial matrix representing the drone, Representing quaternion multiplication, in formula (10) Include , , , In formula (10) Include , .

[0059] In summary, this invention can significantly improve the robustness of UAV swarm movement in dynamic environments. By introducing trajectory optimization methods and designing collision avoidance costs for dynamic and static obstacles, the UAV formation can balance formation maintenance and obstacle collision avoidance in complex and ever-changing obstacle environments, thus ensuring the safety of formation movement.

[0060] The communication network of this invention is implemented by the ROS communication network, which ensures the stability and real-time performance of network communication and provides strong support for the formation movement of UAV swarms.

[0061] This invention has good scalability, allowing the number of drones in a swarm to be added or reduced within the limits of the communication payload; at the same time, this invention can also be applied to unmanned vehicles and unmanned ships.

[0062] The trajectory planning and formation control calculations involved in this invention can be completed using only the onboard computing resources of the cluster, without relying on any external resources. They are applicable to known and unknown environmental application scenarios and have strong adaptability.

[0063] Based on the above embodiments, the present invention also provides a terminal device, the principle block diagram of which can be as follows: Figure 3 As shown, the terminal device includes a processor, memory, network interface, and display screen connected via a system bus. The processor provides computing and control capabilities. The memory includes non-volatile storage media and internal memory. The non-volatile storage media stores the operating system and computer programs. The internal memory provides the environment for the operation of the operating system and computer programs stored in the non-volatile storage media. The network interface is used to communicate with external terminals via a network connection. When the computer program is executed by the processor, it implements a method for planning the trajectory of a drone swarm. The display screen can be an LCD screen or an e-ink screen.

[0064] Those skilled in the art will understand that Figure 3 The schematic diagram shown is only a partial structural diagram related to the present invention and does not constitute a limitation on the terminal device to which the present invention is applied. The specific terminal device may include more or fewer components than shown in the figure, or combine certain components, or have different component arrangements.

[0065] In one embodiment, a terminal device is provided, comprising a memory, a processor, and a drone swarm trajectory planning program stored in the memory and executable on the processor. When the processor executes the drone swarm trajectory planning program, it implements the following operation instructions: Obtain the overall target point that the drone swarm needs to reach, and based on the overall target point, obtain a spline curve. The spline curve is used to characterize the pre-planned trajectory required for the drone swarm to fly to the overall target point. Construct a point set, wherein the nodes in the point set are points selected from the spline curve; Determine the first cost data corresponding to the point set, the first cost data being used to characterize the smoothness of the drone swarm flying along each node in the point set; Determine the second cost data corresponding to the point set, the second cost data being used to characterize the dynamic feasibility data of the UAV swarm flying along each node in the point set; The location of the static obstacle is obtained, and based on the location of the static obstacle and the location of the node, the third cost data corresponding to the point set is determined. The third cost data is used to characterize the risk of the UAV cluster colliding with the static obstacle when flying along each node in the point set. The position and velocity of the dynamic obstacle are obtained, and based on the position and velocity of the dynamic obstacle and the position of the node, the fourth cost data corresponding to the point set is determined. The fourth cost data is used to characterize the risk of the UAV cluster colliding with the dynamic obstacle when flying along each node in the point set. Based on the first cost data, the second cost data, the third cost data, and the fourth cost data corresponding to the point set, the point set is optimized, and the actual flight trajectory of the UAV cluster is constructed through the optimized point set.

[0066] Those skilled in the art will understand that all or part of the processes in the methods of the above embodiments can be implemented by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer-readable storage medium. When executed, the computer program can include the processes of the embodiments of the above methods. Any references to memory, storage, databases, or other media used in the embodiments provided by this invention can include non-volatile and / or volatile memory. Non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM), or flash memory. Volatile memory can include random access memory (RAM) or external cache memory. By way of illustration and not limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), dual data rate SDRAM (DDRSDRAM), enhanced SDRAM (ESDRAM), synchronous link DRAM (SLDRAM), Rambus direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM), and memory bus dynamic RAM (RDRAM), etc.

[0067] Finally, 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 foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features; and these modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A method for trajectory planning of a UAV swarm, the method comprising: The method comprises the following steps: acquiring a total target point to be reached by a UAV cluster, and obtaining a spline curve based on the total target point, the spline curve being used to represent a pre-planned trajectory required for the UAV cluster to fly to the total target point; constructing a point set, the nodes in the point set being points selected from the spline curve; determining first cost data corresponding to the point set, the first cost data being used to represent smoothness of the UAV cluster flying along each node in the point set; determining second cost data corresponding to the point set, the second cost data being used to represent dynamic feasibility data of the UAV cluster flying along each node in the point set; acquiring a position of a static obstacle, and determining third cost data corresponding to the point set based on the position of the static obstacle and the position of the node, the third cost data being used to represent a risk degree of the UAV cluster colliding with the static obstacle when flying along each node in the point set; acquiring a position and a speed of a dynamic obstacle, and determining fourth cost data corresponding to the point set based on the position and the speed of the dynamic obstacle and the position of the node, the fourth cost data being used to represent a risk degree of the UAV cluster colliding with the dynamic obstacle when flying along each node in the point set; optimizing the point set based on the first cost data, the second cost data, the third cost data and the fourth cost data corresponding to the point set, and constructing an actual flight trajectory of the UAV cluster through the point set after optimization. 2.The UAV swarm trajectory planning method of claim 1, wherein, The method for determining the first cost data corresponding to the point set comprises the following steps: acquiring node positions contained in the point set, and acquiring a pre-set flight duration of the UAV cluster between nodes; determining an acceleration of the UAV cluster at the nodes contained in the point set based on each two node positions and the flight duration; determining the first cost data corresponding to the point set based on the acceleration. 3.The UAV swarm trajectory planning method of claim 1, wherein, The method for determining the second cost data corresponding to the point set comprises the following steps: acquiring a pre-set maximum flight speed and a pre-set maximum flight acceleration of the UAV cluster; determining a speed and an acceleration of the UAV cluster at the nodes contained in the point set; determining the second cost data corresponding to the point set based on the maximum flight speed and the maximum flight acceleration and the speed and the acceleration of the node. 4.The method of claim 1, wherein, The method for determining the third cost data corresponding to the point set based on the position of the static obstacle and the position of the node comprises the following steps: acquiring a pre-set safety distance between the nodes contained in the point set and the static obstacle; obtaining an actual distance between the static obstacle and the node based on the position of the static obstacle and the position of the node; determining the third cost data corresponding to the point set based on the safety distance and the actual distance between the static obstacle and the node. 5.The UAV swarm trajectory planning method of claim 1, wherein, The method for determining the fourth cost data corresponding to the point set based on the position and the speed of the dynamic obstacle and the position of the node comprises the following steps: determine a lateral velocity and a longitudinal velocity in the velocity of the dynamic obstacle; determine fourth cost data corresponding to the point set according to the position of the dynamic obstacle, the lateral velocity and the longitudinal velocity, and the positions of the nodes included in the point set. 6.The UAV swarm trajectory planning method of claim 1, wherein, optimize the point set according to the first cost data, the second cost data, the third cost data and the fourth cost data corresponding to the point set, and construct an actual flight trajectory of the UAV cluster by the point set after optimization, comprising: perform weighted calculation on the first cost data, the second cost data, the third cost data and the fourth cost data corresponding to the point set to obtain total cost data of the point set; optimize the point set until the total cost data corresponding to the point set after optimization is minimum; take the position of the point included in the point set corresponding to the minimum total cost data as the position of the control point, and the position of the control point is the position to be worked on by the UAV cluster; construct the actual flight trajectory of the UAV cluster according to the position of the control point.

7. A platoon control method characterized by, comprising: obtain an actual flight trajectory of a UAV cluster, the actual flight trajectory being obtained by a leader based on the UAV cluster trajectory planning method in claim 1, the UAV cluster comprising the leader and a follower; collect a real-time position of the leader in a current time period when the leader flies along the actual flight trajectory; determine a target position of the follower to be reached in the current time period according to a yaw angle and the real-time position of the leader and a preset position of the follower in the formation; collect an actual position of the follower in the current time period, and determine an input of a controller of the follower according to the actual position of the follower and the target position of the follower, the controller being used to make the follower maintain the formation shape based on the input.

8. The platoon control method of claim 7, wherein, The follower and the leader are both quad-rotor UAVs.

9. A terminal device, comprising: The terminal device comprises a memory, a processor, and a UAV cluster trajectory planning program stored in the memory and executable on the processor, and the processor implements the steps of the UAV cluster trajectory planning method in any one of claims 1-6 when executing the UAV cluster trajectory planning program. Alternatively, the terminal device comprises a memory, a processor, and a formation control program stored in the memory and executable on the processor, and the processor implements the steps of the formation control method in any one of claims 7-8 when executing the formation control program.

10. A computer-readable storage medium, characterized in that, The computer readable storage medium stores a UAV cluster trajectory planning program, and the UAV cluster trajectory planning program implements the steps of the UAV cluster trajectory planning method in any one of claims 1-6 when executed by the processor. Alternatively, the computer readable storage medium stores a formation control program, and the formation control program implements the steps of the formation control method in any one of claims 7-8 when executed by the processor.

Images