【 path planning 】 particle swarm based UAV threedimensional path planning MATLAB source code
Particle swarm optimization (PSO) is a numerical optimization algorithm based on swarm intelligence, which was proposed by social psychologist James Kennedy and electrical engineer Russell Eberhart in 1995. Since the birth of PSO, it has been improved in many ways. This section will introduce the basic principle and process of particle swarm optimization algorithm.
2.1 Particle swarm optimization
Particle swarm optimization (PSO) is a population intelligence algorithm, inspired by bird swarm or shoal learning, used to solve nonlinear, nonconvex, or combinatorial optimization problems in many fields of science and engineering.
Figure 1. Russel Eberhart and James Kennedy
2.1.1 Algorithm idea
Many birds are social and form different flocks for a variety of reasons. Flocks may vary in size, occur in different seasons, and may even consist of different species that cooperate well within the group. More eyes and ears means more opportunities to spot food and predators in time. A flock of birds is always beneficial to the survival of its members in many ways:
Foraging: Sociobiologist E.O.Wilson says that, at least in theory, individual members of a group can benefit from other members' discoveries and prior experience in searching for food [1]. If a group of birds have the same food source, then certain species will flock together in a noncompetitive way. That way, more birds can take advantage of other birds' discoveries of food locations.
Defending against Predators: Flocks have many advantages in protecting themselves from predators.
 More ears and eyes means more chances of spotting predators or any other potential danger;
 A flock of birds may confuse or suppress a predator by siege or agile flight;
 In a group, warning each other can reduce the danger to any one bird.
Aerodynamics: When birds fly in groups, they often arrange themselves into specific shapes or formations. Different numbers of birds in a flock, and different airflow generated by each bird flapping its wings, lead to changing wind patterns. These formations take advantage of the different types, allowing flying birds to use the air around them in the most energy efficient way.
The development of particle swarm optimization algorithms requires simulating some of the advantages of bird flocks. However, in order to understand an important property of swarm intelligence and particle swarm optimization, it is worth mentioning some of the disadvantages of bird flocks. It also poses some risks to birds when they flock together. More ears and eyes mean more wings and mouths, which leads to more noise and movement. In this case, more predators can locate the flock and pose a constant threat to the birds. A larger group would also require more food, leading to more competition for food that could eliminate some of the weaker birds in the group. It should be pointed out here that PSO does not have the disadvantage of simulating bird colony behavior, so no individual is allowed to be killed in the search process, while in genetic algorithm, some weaker individuals will die out. In the PSO, all individuals will survive and strive to become stronger throughout the search. In PARTICLE swarm optimization, the improvement of potential solutions is the result of cooperation, while in evolutionary algorithms it is due to competition. This concept makes swarm intelligence different from evolutionary algorithms. In short, in evolutionary algorithms, each iteration has a new population evolving, while in swarm intelligence algorithms, each generation has individuals making themselves better. The identity of the individual does not change with iteration. Mataric[2] gave the following flock rules:
 Safe roaming: when birds fly, there is no collision between each other or with obstacles;
 Dispersal: Each bird keeps a minimum distance from the others;
 Aggregation: Each bird will also maintain a maximum distance from the other birds;
 Homing: All birds are likely to find a food source or nest.
In the design of particle swarm optimization algorithm, these four rules are not used to simulate the flock behavior of birds. In the elementary particle swarm optimization model developed by Kennedy and Eberhart, the motion of the agent does not follow the rules of safe roaming and dispersion. In other words, the agents in the pSO algorithm are allowed to be as close to each other as possible during the motion of the elementary PSO algorithm. Aggregation and homing are effective in the particle swarm optimization model. In particle swarm optimization, agents must fly within a specific region in order to maintain maximum distance from any other agents. This is equivalent to the search staying within or at the boundaries of the search space throughout the process. The fourth rule, homing, means that any agent in the group is globally optimal.
During the development of the PSO model, Kennedy and Eberhart proposed five basic principles for judging whether a group of agents is a group:
 Proximity principle: The agency group should be able to perform simple spatial and temporal calculations;
 The principle of quality: the agency group can respond to the quality factors in the environment;
 The principle of multiple responses: agent groups should not operate in too narrow channels;
 The principle of stability: the agent group cannot change its behavior pattern every time the environment changes;
 The principle of adaptability: the agent group can change its behavior pattern when the computational cost is small.
2.1.2 Particle swarm optimization process
With these five principles in mind, Kennedy and Eberhart developed a PSO model for function optimization. In particle swarm optimization algorithm, random search method and swarm intelligence are used to solve the problem. In other words, particle swarm optimization is a population intelligent search algorithm. The search is done by a randomly generated set of possible solutions. This set of possible solutions is called a group, and each possible solution is called a particle.
In particle swarm optimization (PSO), particle search is affected by two learning methods. Each particle is learning from the others and learning its own experiences as it moves. Learning from others can be called social learning, while learning from one's own experience can be called cognitive learning. As a result of social learning, the particle stores in its memory the best solution of access for all particles in the group, which we call GBEST. Through cognitive learning, the particle stores in its memory the best solution it has visited so far, called pBest.
The change in direction and size of any particle is determined by a factor called velocity, which is the rate of change in position with respect to time. For PSO, time is iterated. Thus, for particle swarm optimization, velocity can be defined as the rate of change of position relative to iteration. As the iteration counter unit increases, velocity V has the same dimension as position x.
For the Ddimensional search space, The ith particle in the population at the time step T is divided by the D dimension vector x I t = (x I 1 t,..., x I D t) t x_i^t = {(x_{i1}^t, \cdots,x_{iD}^t)^ t}xit=(xi1t,...,xiDt) t The velocity is represented by another D dimension vector V I t=(v I 1t,...,v I Dt) t v_i^t = {(v_{i1}^t, \cdots,v_{iD}^t)^ t}vit=(vi1t,... viDt) t. P I t = (p I 1 t,... p I D t) t p_i^t = {\left({p_{i1}^t, \cdots,p_{iD}^t} \right)^ t} pit=(pi1t,...,piDt) t indicates that the index of the optimal particle in the population is "g". The velocity and position of the ith particle are updated by the following formula:
V I d t + 1 = v I d t + c 1 r 1 (p I d t − x I d t) + c 2 r 2 (p g d t − x I d t) (1) v_{id}^{t + 1} = v_{id}^t + {c_1}{r_1}\left( {p_{id}^t  x_{id}^t} \right) + {c_2}{r_2}\left( {p_{gd}^t  x_{id}^t} \right)\tag 1 + 1 = vidt vidt + c1r1 (pidt  xidt) + c2r2 (PGDT  xidt) (1)
x i d t + 1 = x i d t + v i d t + 1 (2) x_{id}^{t + 1} = x_{id}^t + v_{id}^{t + 1}\tag 2xidt+1=xidt+vidt+1(2)
Where d = 1, 2,... D is the dimension, I =1,2... ,S is the particle index,S is the population size. C1 and C2 are constants, known as the cognitive and social scaling parameters, or simply as the acceleration coefficients. R1 and r2 are random numbers satisfying the uniform distribution [0,1]. The two formulas above update each dimension of each particle individually, and the only connection between different dimensions in the problem space is introduced by the objective function, namely the best positions gbest and pbest[3] found at present. The algorithm process of PSO is as follows:
Algorithm 1: basic particle swarm optimization algorithm
2.1.3 Interpretation of the update equation
The right side of velocity update equation (1) includes three parts (3) :
 The velocity of the previous time, v, can be thought of as a momentum term, which stores the previous direction of motion in order to prevent the particle from drastically changing direction.
 The second is the cognition or ego section, through which the particle's current position moves towards its own best position, so that the particle will remember its best position throughout the search and avoid wandering around. It should be noted here that pidtxIDt is a vector from XIDT to PIDt, thus attracting the current position to the best position of the particle. The order of the two cannot be changed, otherwise the current position will be far away from the best position.
 The third is the social section, which is responsible for sharing information through groups. Through this, the particle moves to the optimal individual in the group, that is, each individual learns from the other individuals in the group. Again, both should be PGbtXIDT.
It can be seen that the cognitive scale parameter C1 adjusts the maximum stride length in the direction of the particle's optimal position, while the social scale parameter C2 adjusts the maximum stride length in the direction of the globally optimal particle. Figure 2 shows the typical geometry of particle motion in two dimensional space.
Figure 2. Geometric illustration of particle movement in particle swarm optimization
From the updated equation, it can be seen that Kennedy and Eberhart's PSO design follows the five basic principles of PSO. In the process of particle swarm optimization, a series of time steps are calculated in D  dimensional space. At any time step, the population follows the guiding direction of GBEST and PBest, that is, the population responds to the quality factors, thus following the quality principle. Since there are uniformly distributed random numbers R1 and R2 in the velocity update equation, which are randomly assigned at the current position between pbest and GBest, this proves the diversity of response principles. In the process of particle swarm optimization (PSO), only when the PSO receives good information from GBEST, will the random motion occur, thus proving the stability principle of pSO process. The population changes when THE GBEST changes, so the adaptive principle is followed.
2.2 Parameters in PARTICLE swarm optimization
The convergence rate and optimization ability of any populationbased algorithm are affected by its parameter selection. In general, since the parameters of these algorithms are highly dependent on the problem parameters, it is not possible to give general recommendations for setting the parameters of these algorithms. However, existing theoretical and/or experimental studies give a general range of parameter values. Similar to other populationbased search algorithms, the parameter adjustment of general PSO is always a challenging task due to the random factors R1 and R2 in the search process. The base version of PSO requires very few parameters. This chapter only discusses the parameters of the base version of PSO introduced in [4].
A basic parameter is the group size, which is usually set empirically based on the number of decision variables in the problem and the complexity of the problem. 2050 particles are generally recommended.
The other parameter is the scaling factors C1 and c2. As mentioned earlier, these parameters determine the step size of the particle in the next iteration. In other words, c1 and C2 determine the velocity of the particle. In the base version of PSO, select c1=c2=2. In this case, the increase in the velocity of particle S is uncontrolled, which is conducive to a faster convergence rate, but not conducive to a better use of the search space. If we set c1=c20, then the particle will attract the average of pBest and gBest. A c1 C2 setting favors multimode problems, while C2 C1 favors singlemode problems. In the search process, the smaller the value of C1 and C2, the smoother the particle trajectory, while the larger the value of C1 and C2, the more violent the particle motion, the greater the acceleration. Researchers have also proposed an adaptive acceleration coefficient [5].
The stop criterion is not only a parameter of pSO, but also a parameter of any metaheuristic algorithm based on population. Commonly used stop criteria are usually based on the maximum number of functions evaluated or iterated, which is proportional to the time spent by the algorithm. A more effective stopping criterion is based on the search ability of the algorithm, if an algorithm does not significantly improve the solution within a certain number of iterations, then the search should be stopped.
3.1 code
reference

O. Wilson, E., Sociobiology: The New Synthesis. 1976.

J Matari 'c, M. and A. Brooks, Interaction and Intelligent Behavior. 1999.

Trelea, I.C., The particle swarm optimization algorithm: convergence
analysis and parameter selection. Information Processing Letters, 2003.
85(6): p. 317325. 
Kennedy, In Proceedings of ICNN '95International Conference on Neural Networks. J. And R. Eberhart Networks. 1995.

Zhan, Z., et al. Adaptive control of acceleration coefficients for particle
swarm optimization based on clustering analysis. in 2007 IEEE Congress on
Evolutionary Computation. 2007.