基于粒子算法的二维路径规划算法设计与仿真

基于粒⼦算法的⼆维路径规划算法设计与仿真

⽬录

1 粒⼦算法

对⼩结如下：粒⼦算法是⼀种基于种的搜索过程，其中每个个体（鸟）称为粒⼦，定义为在D维搜索空间中待优化问题的可⾏解，保存有其历史最优位置和所有的最优位置的记忆，以及速度。在每⼀演化代，粒⼦的信息被组合起来调整速度关于每⼀维上的分量，继⽽被⽤来计算新的粒⼦位置。粒⼦在多维搜索空间中不断改变它们的状态，直到到达平衡或最优状态，或者超过了计算限制为⽌。的不同维度之间唯⼀的联系是通过⽬标函数引⼊的。很多经验证据已经显⽰该算法是⼀个⾮常有效的优化⼯具：

在此不对粒⼦算法原理公式进⾏介绍说明。直接上代码：

function [ GBEST , cgCurve ] = PSO ( noP, maxIter, problem, dataVis )

% Define the details of the objective function

nVar = problem.nVar;

ub = problem.ub;

lb = problem.lb;

fobj = problem.fobj;

% Extra variables for data visualization

average_objective = zeros(1, maxIter);

cgCurve = zeros(1, maxIter);

FirstP_D1 = zeros(1 , maxIter);

position_history = zeros(noP , maxIter , nVar );

% Define the PSO's paramters

wMax = 0.9;

wMin = 0.2;

buckboost电路c1 = 2;

c2 = 2;

vMax = (ub - lb) .* 0.2;

vMin = -vMax;

% The PSO algorithm

% Initialize the particles

for k = 1 : noP

Swarm.Particles(k).X = (ub-lb) .* rand(1,nVar) + lb;

Swarm.Particles(k).V = zeros(1, nVar);

Swarm.Particles(k).PBEST.X = zeros(1,nVar);

Swarm.Particles(k).PBEST.O = inf;

Swarm.GBEST.X = zeros(1,nVar);

Swarm.GBEST.O = inf;

end

% Main loop

for t = 1 : maxIter

% Calcualte the objective value

for k = 1 : noP

currentX = Swarm.Particles(k).X;

position_history(k , t , : ) = currentX;

Swarm.Particles(k).O = fobj(currentX);

average_objective(t) = average_objective(t) + Swarm.Particles(k).O;

% Update the PBEST

if Swarm.Particles(k).O < Swarm.Particles(k).PBEST.O

Swarm.Particles(k).PBEST.X = currentX;

Swarm.Particles(k).PBEST.O = Swarm.Particles(k).O;

end

% Update the GBEST

if Swarm.Particles(k).O < Swarm.GBEST.O

Swarm.GBEST.X = currentX;

Swarm.GBEST.O = Swarm.Particles(k).O;

end

% Update the X and V vectors

w = wMax - t .* ((wMax - wMin) / maxIter);

FirstP_D1(t) = Swarm.Particles(1).X(1);

for k = 1 : noP

Swarm.Particles(k).V = w .* Swarm.Particles(k).V + c1 .* rand(1,nVar) .* (Swarm.Particles(k).PBEST.X - Swarm.Particles(k).X) ... + c2 .* rand(1,nVar) .* (Swarm.GBEST.X - Swarm.Particles(k).X);

% Check velocities

index1 = find(Swarm.Particles(k).V > vMax);

index2 = find(Swarm.Particles(k).V < vMin);

Swarm.Particles(k).V(index1) = vMax(index1);

Swarm.Particles(k).V(index2) = vMin(index2);

Swarm.Particles(k).X = Swarm.Particles(k).X + Swarm.Particles(k).V;

% Check positions

index1 = find(Swarm.Particles(k).X > ub);

index2 = find(Swarm.Particles(k).X < lb);

Swarm.Particles(k).X(index1) = ub(index1);

Swarm.Particles(k).X(index2) = lb(index2);

end

if dataVis == 1

outmsg = ['Iteration# ', num2str(t) , ' Swarm.GBEST.O = ' , num2str(Swarm.GBEST.O)];

disp(outmsg);

end

cgCurve(t) = Swarm.GBEST.O;

average_objective(t) = average_objective(t) / noP;

fileName = ['Resluts after iteration # ' , num2str(t)]; save( fileName)

end

GBEST = Swarm.GBEST;

if dataVis == 1

iterations = 1: maxIter;

%% Draw the landscape

figure

x = -50 : 1 : 50;

y = -50 : 1 : 50;

[x_new , y_new] = meshgrid(x,y);

吸油烟机止回阀for k1 = 1: size(x_new, 1)

for k2 = 1 : size(x_new , 2)

X = [ x_new(k1,k2) , y_new(k1, k2) ];

z(k1,k2) = ObjectiveFunction( X );

end

subplot(1,5,1)

surfc(x_new , y_new , z);

title('Search landscape')

xlabel('x_1')

ylabel('x_2')

zlabel('Objective value')

shading interp

camproj perspective

box on

set(gca,'FontName','Times')

%% Visualize the cgcurve

subplot(1,5,2);

semilogy(iterations , cgCurve, 'r');

title('Convergence curve')

xlabel('Iteration#')

ylabel('Weight')

%% Visualize the average objectives

subplot(1,5,3)

semilogy(iterations , average_objective , 'g')

title('Average objecitves of all particles')

xlabel('Iteration#')

ylabel('Average objective')

%% Visualize the fluctuations

subplot(1,5,4)

plot(iterations , FirstP_D1, 'k');

title('First dimention in first Particle')

xlabel('Iteration#')

ylabel('Value of the first dimension')

%% Visualize the search history

subplot(1,5,5)

hold on

for p = 1 : noP

for t = 1 : maxIter

x = position_history(p, t , 1);

y = position_history(p, t , 2);

myColor = [0+t/maxIter 0 1-t/maxIter ];

接种棒plot(x , y , '.' , 'color' , myColor );

end

contour(x_new , y_new , z);

plot(Swarm.GBEST.X(1) , Swarm.GBEST.X(2) , 'og');

xlim([lb(1) , ub(1)])

ylim([lb(2) , ub(2) ])

title('search history')

xlabel('x')

ylabel('y')

box on

set(gcf , 'position' , [128 372 1634 259])

end

2 ⼆维环境建模

⼆维环境建模可以看作是对现实三维环境的投影和简化，假设我们的运动物⼯作在相对⽔平的环境，且匀速运动。建模⽰意图如下：

支的结构上图中，S,T分别是起点和终点，圆圈区域是我们路径规划需要尽量避开的区域（亦称为威胁），其主要参数为威胁中⼼，威胁半径和威胁等级（威胁等级并没有在⽰意图显⽰），以黄⾊节点为waypoint的线条是可⾏的⼀条规划路径。

通过设计复杂程度不⼀样的⼆维环境，来测试算法在不同苛刻程度下的环境的效果。在此⽤了三个⼆维环境来进⾏实验：

%% case 1

radius = [10 10 8 12 9];

threat_lv = [2 10 1 2 3];

threat_center = [45,52;12,40;32,68;36,26;55,80];

%% case 2

radius = [10 10 8 12 9 10 10];

threat_lv = [2 10 1 2 5 2 4];

threat_center = [52,52;32,40;12,48;36,26;63,56;50,42;30,70];

%% case 3

radius = [10 18 10 12 10 11];

threat_lv = [5 5 5 5 5 5];

threat_center = [30,20;50,15;65,55;50,80;75,90;50,36];琴谱架

3 路径规划

由于起点S和终点T的连线并不与X轴平⾏，为了⽅便计算我们先将坐标轴旋转，将线段ST所在的直线作为新的X轴，S作为新坐标系的原点。

旋转矩阵：

路径规划算法设计：我们将新的X轴ST等分为D+1份，意味着路径包含D个黄⾊节点，D个黄⾊节点在新坐标系下的X坐标是已知的，⽽Y坐标是需要通过灰狼算法求解的。因此灰狼算法中的每个可⾏解都是D维的（即包含D个黄⾊节点的Y坐标）。

3.1 初始化

粒⼦算法需要设置⼏个初始参数，如种数量N（随机⽣成多少个可⾏解）、迭代次数、解的维数D，以及上下界。

种数量N可根据实际问题复杂程度或搜索空间⼤⼩来设置，显然N越⼤不⼀定越好，因为会增加算法时间复杂度。

最⼤迭代次数不宜过⼩，可通过多次实验整定。

上下界(lb,ub)限定了算法在整个过程中⽣成的解的范围，如果我们不设定上下界，那么最后⽣成的路径可能⾮常的离谱，虽然它没有经过威胁区域，但覆盖的范围可能会⾮常⼤，不满⾜实际要求，加上下界可以限定路径的⼤概区域。

3.2 适应度函数

这⾥我们将路径成本定义为威胁代价的累加，带价越低意味着适应度越好，只有当路径经过了威胁区域时，我们才计算该段掉在威胁区域的路径代价：

锻造操作机如上图所⽰，威胁代价最终变成五个采样点的威胁代价的加权和。

3.3 仿真结果

case1

本文发布于:2024-09-23 00:21:44，感谢您对本站的认可！

本文链接：https://www.17tex.com/tex/3/311428.html

上一篇：局部路径规划-01曲线插值法

下一篇：自动驾驶路径规划算法学习-RRT算法及matlab实现

标签：威胁路径算法环境区域代价需要规划

留言与评论（共有 0 条评论）