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

最新推荐文章于 2024-06-08 10:31:00 发布

早餐两个叉烧包

最新推荐文章于 2024-06-08 10:31:00 发布

阅读量2.9k

点赞数 1

文章标签：算法机器学习 matlab

本文链接：https://blog.csdn.net/u011499347/article/details/122631843

版权

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;
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
    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
    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
    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作为新坐标系的原点。

旋转矩阵： $R=\begin{bmatrix} cos\Theta &sin\Theta \\-sin\Theta & cos\Theta \end{bmatrix}$

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

3.1 初始化

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

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

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

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

3.2 适应度函数

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

如上图所示，威胁代价最终变成五个采样点的威胁代价的加权和。

3.3 仿真结果

case1

case2

case3

上图中生成的三条路径要求也是不能有冲突，路径除了起点和终点之外不能有交集。我的解决方案是为三条路径分配不同上下界，保证区域不会重合，从根本上保证三条路径不会有交集。

可以看到仿真图里有些路径经过了威胁区域，这是由粒子群算法的随机性和上下界造成的。由于种群初始化是随机生成的，而初始化种群是会影响最终的算法结果的，这种随机性有可能让算法陷入局部最优，而算法的勘探机制越好，跳出局部最优的能力也会越强，但不可避免，这也是目前群智能算法的主要弊端，但是全局搜索又是不现实的，搜索整个解空间很浪费时间。

早餐两个叉烧包

关注

1
点赞
踩
31

收藏

觉得还不错? 一键收藏
3
评论
基于粒子群算法的二维路径规划算法设计与仿真

目录1 粒子群算法2 二维环境建模3 路径规划3.1 初始化3.2 适应度函数3.3 仿真结果1 粒子群算法对粒子群算法小结如下：粒子群算法是一种基于种群的搜索过程，其中每个个体（鸟）称为粒子，定义为在D维搜索空间中待优化问题的可行解，保存有其历史最优位置和所有粒子的最优位置的记忆，以及速度。在每一演化代，粒子的信息被组合起来调整速度关于每一维上的分量，继而被用来计算新的粒子位置。粒子在多维搜索空间中不断改变它们的状态，直到到达平衡或最优状态，或者超过了计算限制..
复制链接

扫一扫