Path planning algorithm: path planning algorithm based on backtracking search optimization - with code
Article directory
Abstract: This paper mainly introduces the use of intelligent optimization algorithm backtracking search algorithm for path planning.
1. Algorithm principle
For the principle of backtracking search algorithm, please refer to: https://blog.csdn.net/u011835903/article/details/108366901
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.
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 ( P a t h ) = ∑ i = 0 n − 1 ( x l i + 1 − x l i ) 2 + ( y l i + 1 − y l i ) 2 (1) L(Path) = \sum_{i=0}^{n-1}\sqrt{(xl_{i+1} - xl_i)^2 + (yl_{i+1} - yl_{i})^2}\tag{1} L(Path)=i=0∑n−1(xli+1−xli)2+(yli+1−yli)2( 1 )
where (x,y) are the coordinates of the middle point of the path
Use the backtracking search algorithm to optimize the above formula and find the shortest path. The backtracking search algorithm parameters 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.