UAV swarms have always been a hot and difficult research topic. However, current related cluster planning strategies usually lack the ability to avoid obstacles in cluttered environments. In order to solve the problem of UAV formation obstacle avoidance flight in a dense and complex environment, Zhejiang University Fast Lab proposed a distributed UAV cluster trajectory optimization algorithm - Swarm-Formation. Relying on this set of algorithms, UAVs can plan collision -free UAV formation trajectories in dense environments , ensuring collision-free trajectory generation during formation flight.
The algorithm proposes a new differentiable metric to quantify the overall similarity distance between formations, and then formulates this metric into an optimization framework that implements spatiotemporal planning using polynomial trajectories capable of minimizing collision penalties to simultaneously address formation keeping and obstacle avoidance. The Swarm-Formation algorithm provides UAV swarm researchers and engineers with a new idea for UAV swarm obstacle avoidance.
SwarmFormation algorithm comparison
The video comes from the Internet
Compared with other methods, the optimization method of this algorithm has achieved good performance in position error and formation similarity error . Because the proposed formation similarity metric method provides high flexibility for generating trajectories in terms of scaling and rotation invariance, even in dense environments, there is a high success rate to plan and generate collision-free formation trajectories.
Analysis of SwarmFormation Algorithm
The algorithm comes from the paper "Distributed Swarm Trajectory Optimization for Formation Flight in Dense Environments" from the Fast Lab of Zhejiang University (see the end of the paper for the address of the paper and code), which proposes a distributed cluster trajectory optimization method for formation flight in a dense environment, using a graph-based differentiable metric to quantify the distance between UAV formations. The trajectory generation of UAV formation flight is attributed to an optimization problem, and a collision-free smooth trajectory that maintains formation flight is obtained by minimizing the cost function. The cost function consists of seven objective functions and their corresponding weights, which are smoothness penalty, total time penalty, collision item penalty, cluster formation penalty, inter-machine collision penalty, dynamic feasibility penalty and uniform distribution penalty of constraint points.
smoothness penalty
The trajectory of the UAV is represented by a high-order polynomial, and the third-order derivative jerk of the trajectory is used as the control input. Minimizing the high-order derivative can make the entire trajectory control energy small. Therefore, using the square of the jerk of the global trajectory as a penalty can make the generated UAV trajectory control energy small and make the trajectory smooth.
total time penalty
For UAV swarm formation flight, we hope that the weighted total execution time of the formation will be shorter, that is, the formation can achieve the planning goal faster.
collision penalty
The Euclidean Signed Distance Field (ESDF) can calculate the distance information and gradient information between the UAV and the obstacle. When the distance between the constraint point of the UAV trajectory and the nearest obstacle is less than the safety distance, the generated UAV trajectory will be pushed away from the obstacle.
Group Formation Penalty
The undirected graph is used to model the UAV formation. Each UAV is the vertex of the undirected graph. The edges of the undirected graph indicate that the UAVs can measure the geometric distance between each other, and each UAV can communicate with other UAVs. Define a formation similarity distance metric, which is the normalized Laplacian matrix of the current formation undirected graph minus the normalized Laplacian matrix of the expected formation, which is used to evaluate the difference between the UAV formation and the expected formation in the 3D workspace. By punishing the formation similarity distance measure, the UAV cluster can be as close as possible to the desired formation. It should be noted that the translation and rotation of the UAV formation will not affect the topology between UAVs, and since the Laplacian matrix of the undirected graph is normalized, the translation, selection, and scaling of the UAV formation will not affect the formation similarity distance measure.
collision penalty
In this algorithm, since the non-cooperative framework is adopted, it means that UAVs will receive the trajectories of other UAVs as constraints to generate their own trajectories. And by keeping the constraint point away from other drone trajectories at the global time stamp, the trajectories of the drones will not collide with each other.
dynamic feasibility penalty
Limit the trajectory derivative of the drone, that is, the amplitude of the velocity, acceleration, and jerk of the drone, to ensure that the trajectory generated by the algorithm can allow the drone to execute smoothly.
Uniform distribution of constraint points
The generated UAV trajectory contains multiple constraint points, and the UAV constraint points should be uniform in space. Non-uniformly constrained points may jump over some smaller obstacles, which may reduce the safety of the trajectory. This algorithm achieves uniform distribution of constraint points by penalizing the variance of the squared distance between each pair of adjacent constraint points.
The above seven objective functions plus its weight can calculate the cost of a certain trajectory . The minimum cost means that this trajectory is the optimal trajectory . According to the actual situation and different application scenarios, the corresponding weight value should also be adjusted appropriately.
Recently, we also combined the open-source ROS function package of this algorithm with PX4 simulation , and reproduced the algorithm on the gazebo simulation platform and achieved good performance. The following is the simulation test video of our PX4 UAV application Swarm Formation:
Related papers and code
Paper: https://arxiv.org/pdf/2109.07682.pdf
Code address: https://github.com/ZJU-FAST-Lab/Swarm-Formation
- END -