matlab粒子群优化算法路径规划代码解读

2023-05-16

前言

粒子群算法是一种群智能优化算法，该算法具有原理简单、易实现、控制参数较少等优点，下面根据Yarpiz公司的matlab代码就其在路径规划中的应用进行简单的介绍，以供读者更好的理解粒子群优化算法的实际应用。

代码结构

01 pso函数

02 CreateModel函数

03 MyCost函数

04 ParseSolution函数

05 CreateRandomSolution函数

06 PlotSolution函数

CreateModel函数

该函数的功能是创建路径规划所需要的地图模型，模型中包含了起点，终点以及障碍物的位置，在该工程中，障碍物统一用圆形表示。

该函数的返回值为一个关于model的结构体，其中包含了地图模型的全部信息，具体的代码如下：

%
% Copyright (c) 2015, Yarpiz (www.yarpiz.com)
% All rights reserved. Please read the "license.txt" for license terms.
%
% Project Code: YPAP115
% Project Title: Path Planning using PSO in MATLAB


function model=CreateModel()

    % 设置起点坐标
    xs=0;
    ys=0;
    
    % 设置终点坐标
    xt=4;
    yt=6;
    
    % 圆形障碍物
    xobs=[1.5 3.5 1.2]; % 设置障碍物的x坐标
    yobs=[4.0 3.0 1.5];% 设置障碍物的y坐标
    robs=[1.0 0.6 0.8];% 设置障碍物的半径
    
    n=3; % 设置种群中粒子个数
    
    % 设置地图范围
    xmin=-10; 
    xmax= 10;
    
    ymin=-10;
    ymax= 10;
    
    % 将参数存入结构体
    model.xs=xs;
    model.ys=ys;
    model.xt=xt;
    model.yt=yt;
    model.xobs=xobs;
    model.yobs=yobs;
    model.robs=robs;
    model.n=n;
    model.xmin=xmin;
    model.xmax=xmax;
    model.ymin=ymin;
    model.ymax=ymax;
    
end

CreateRandomSolution函数

该函数的功能主要是在地图空间内随机生成粒子种群的位置，用于粒子群的随机初始化

函数的输入为地图模型，输出为随机粒子群的位置，具体的代码如下：

function sol1=CreateRandomSolution(model)
    %种群内粒子数量
    n=model.n;
    
    %记录地图大小
    xmin=model.xmin;
    xmax=model.xmax;
    
    ymin=model.ymin;
    ymax=model.ymax;

    %在地图内生成随机粒子
    sol1.x=unifrnd(xmin,xmax,1,n);
    sol1.y=unifrnd(ymin,ymax,1,n);
    
end

ParseSolution函数

该函数的功能是分析粒子解的可行性，根据粒子解中的粒子进行曲线拟合，并判断拟合曲线是否与障碍物相交，如果不与障碍物相交，则该解为可行性解。

函数的输入为待分析的粒子解与地图模型，输出为分析后的结果，具体的代码如下：

function sol2=ParseSolution(sol1,model)

    x=sol1.x;
    y=sol1.y;
    
    xs=model.xs;
    ys=model.ys;
    xt=model.xt;
    yt=model.yt;
    xobs=model.xobs;
    yobs=model.yobs;
    robs=model.robs;
    
    % 待拟合的点坐标（包含起点，终点和粒子解）
    XS=[xs x xt];
    YS=[ys y yt];
    k=numel(XS);
    TS=linspace(0,1,k);
    %曲线拟合
    tt=linspace(0,1,100);
    xx=spline(TS,XS,tt);%先找到TS与XS的函数关系，再将该函数关系映射到tt上
    yy=spline(TS,YS,tt);
    
    %求曲线差分
    dx=diff(xx);
    dy=diff(yy);
    
    %根据差分结果计算线路总长度
    L=sum(sqrt(dx.^2+dy.^2));
    
    %判断是否与障碍物相交
    nobs = numel(xobs); % Number of Obstacles
    Violation = 0;
    for k=1:nobs
        d=sqrt((xx-xobs(k)).^2+(yy-yobs(k)).^2); %计算线路上的点与圆心的距离
        v=max(1-d/robs(k),0);   %如果该值为正，则说明相交
        Violation=Violation+mean(v);
    end
    
    %保存分析结果并输出
    sol2.TS=TS;
    sol2.XS=XS;
    sol2.YS=YS;
    sol2.tt=tt;
    sol2.xx=xx;
    sol2.yy=yy;
    sol2.dx=dx;
    sol2.dy=dy;
    sol2.L=L;
    sol2.Violation=Violation;
    sol2.IsFeasible=(Violation==0);
    
end

MyCost函数

该函数主要是根据粒子解的分析结果计算粒子解的质量，在线路与障碍物不相交的情况下，线路的长度越小，解得质量越高。

该函数的输入为待分析的粒子解与地图模型，输出是分析结果与该条线路的成本，线路的成本越小，解的质量越高，具体的代码为

function [z, sol]=MyCost(sol1,model)

    %粒子解的分析
    sol=ParseSolution(sol1,model);
    
    %惩罚系数，该值越大，对障碍物越敏感
    beta=100;
    %计算线路的成本
    z=sol.L*(1+beta*sol.Violation);

end

pso函数

pso函数的主要功能就是依次调用以上函数，以依次实现以下功能：

（1）初始化地图模型

（2）初始化所有粒子群的速度和位置，并初始化粒子的历史最优解与群体最优解

（3）在迭代过程中，计算各个粒子群生成的线路成本，并及时更新粒子的历史最优解与群体最优解。

（4）对每个粒子的的速度和位置进行更新，并且限定位置与速度的最大最小值，防止粒子越界。

（5）运行结束后输出最优结果。

pso函数迭代过程中的核心部分为更新粒子的速度与位置，其相应部分的代码为：

    for i=1:nPop
        
        %更新x方向的速度 particle(i)为第i个种群的粒子
        particle(i).Velocity.x = w*particle(i).Velocity.x ...
            + c1*rand(VarSize).*(particle(i).Best.Position.x-particle(i).Position.x) ...
            + c2*rand(VarSize).*(GlobalBest.Position.x-particle(i).Position.x);
        
        %对x方向的粒子速度进行限幅 
        particle(i).Velocity.x = max(particle(i).Velocity.x,VelMin.x);
        particle(i).Velocity.x = min(particle(i).Velocity.x,VelMax.x);
        
        %对粒子x方向的位置进行更新
        particle(i).Position.x = particle(i).Position.x + particle(i).Velocity.x;
        
        %当粒子的位置越界时,另其速度反向，防止其再次越界
        OutOfTheRange=(particle(i).Position.x<VarMin.x | particle(i).Position.x>VarMax.x);
        particle(i).Velocity.x(OutOfTheRange)=-particle(i).Velocity.x(OutOfTheRange);
        
        %对粒子x方向的位置进行限幅
        particle(i).Position.x = max(particle(i).Position.x,VarMin.x);
        particle(i).Position.x = min(particle(i).Position.x,VarMax.x);
        
        
        %更新y方向的速度 particle(i)为第i个种群的粒子
        particle(i).Velocity.y = w*particle(i).Velocity.y ...
            + c1*rand(VarSize).*(particle(i).Best.Position.y-particle(i).Position.y) ...
            + c2*rand(VarSize).*(GlobalBest.Position.y-particle(i).Position.y);
        
        %对y方向的粒子速度进行限幅 
        particle(i).Velocity.y = max(particle(i).Velocity.y,VelMin.y);
        particle(i).Velocity.y = min(particle(i).Velocity.y,VelMax.y);
        
        %对粒子y方向的位置进行更新
        particle(i).Position.y = particle(i).Position.y + particle(i).Velocity.y;
        
        %当粒子的位置越界时,另其速度反向，防止其再次越界
        OutOfTheRange=(particle(i).Position.y<VarMin.y | particle(i).Position.y>VarMax.y);
        particle(i).Velocity.y(OutOfTheRange)=-particle(i).Velocity.y(OutOfTheRange);
        
        %对粒子y方向的位置进行限幅
        particle(i).Position.y = max(particle(i).Position.y,VarMin.y);
        particle(i).Position.y = min(particle(i).Position.y,VarMax.y);
        
        % 计算线路的成本
        [particle(i).Cost, particle(i).Sol]=CostFunction(particle(i).Position);
        
        % 更新群体最优值
        if particle(i).Cost<particle(i).Best.Cost
            
            particle(i).Best.Position=particle(i).Position;
            particle(i).Best.Cost=particle(i).Cost;
            particle(i).Best.Sol=particle(i).Sol;
            
            % 更新全局最优值
            if particle(i).Best.Cost<GlobalBest.Cost
                GlobalBest=particle(i).Best;
            end
            
        end
        
        
    end

PlotSolution函数

该函数只要是将运算的结果进行输出

运行结果

运行的结果为：

后记

该代码中仅实现了标准的PSO，PSO主要的是在处理多峰问题时容易过早收敛，使PSO算法陷入局部最优解，这主要是由于粒子群在迭代过程中多样性快速丢失造成的，目前粒子群算法的优化方法多种多样，读者可以在标准PSO算法的基础上进行改进优化，以达到想要的结果。

本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)