Path planning algorithm: Path planning algorithm based on particle swarm optimization - with code

Path planning algorithm: Path planning algorithm based on particle swarm optimization - with code

Abstract: This paper mainly introduces the use of intelligent optimization algorithm particle swarm algorithm for path planning.

1. Algorithm principle

Please refer to the blog for the specific principle of the particle swarm algorithm

1.1 Environment settings

In the path optimization of mobile robots, the solution of each optimization algorithm represents a motion path of the robot. The optimization algorithm will find an optimal path among many paths through optimization calculation.
The setting of the optimization algorithm must correspond to the robot motion environment model. Without loss of generality, assume that the result obtained after using the grid method to model the robot's motion environment is an m×n rectangular area, and the coordinate values ​​start from 1, as shown in Figure 1. The coordinate origin grid represents the initial position of the robot, and the grid corresponding to the coordinates (m, n) represents the moving target position of the robot. An important content of the optimization algorithm setting is to determine the mathematical expression form of the optimization algorithm. Here, this problem is transformed into representing the moving path of the robot with a vector. After analysis, it is found that although the model established by the grid method discretizes the space, the moving path of the robot is still continuous in essence.

Figure 1. Raster map

1.2 Constraints

For the path optimization of the robot, its movement path must be limited in the grid space, that is, the search cannot cross the rectangular boundary of the grid. In addition, it should be restricted by obstacles, that is, the trajectory of the robot cannot pass through the grid area where obstacles exist.

1.3 Fitness function

In the modeling method of this paper, the path planning goal of this paper is the shortest path length. The length of the path can be expressed as:

$$L(Path)=\sum _{i=0}^{n-1}\sqrt{(x{l}_{i+1}-x{l}_i{)}^2+(y{l}_{i+1}-y{l}_i{)}^2}\text{\hspace{1em}( 1 )}$$
where (x,y) are the coordinates of the middle point of the path

Use the particle swarm algorithm to optimize the above formula and find the shortest path. The parameters of the particle swarm algorithm are set as follows:

%% 粒子群算法参数设置
dim=length(noLM);%维度，即为非障碍物个数。
numLM0=round((EndPoint(1)-StartPoint(1))/4);%每次迭代选取的的中间路径点个数，可调
lb=0;%下边界
ub=1;%上边界
Max_iteration = 100;%最大迭代次数
SearchAgents_no = 30;%种群数量
fobj = @(x)fun(x,noS,noE,numLM0,net);%适应度函数

2. Algorithm results

3. MATLAB code

In this program, 1. Maps are supported to be created and saved arbitrarily. 2. In fact, you can change it arbitrarily.

4. References

[1] Luo Yangyang, Peng Xiaoyan. Global path planning for four-wheeled mobile robots based on improved PSO [J]. Computer Simulation, 2020,37(07):373-379.

[2] Lu Dan. Application Research of Particle Swarm Algorithm in Path Planning of Mobile Robots [D]. Wuhan University of Science and Technology, 2009.

-379.

[2] Lu Dan. Application Research of Particle Swarm Algorithm in Path Planning of Mobile Robots [D]. Wuhan University of Science and Technology, 2009.