pso.m

一个用粒子群做的路径规划。功能比较简单

%------基本粒子群优化算法（Particle Swarm Optimization）-----------
function pg = PSO( void )
%--------声明全局---------------------------
global N;
global m;
global MaxDT;
global Qmax;
global Kmax;
global w;
global c1;
global c2;
global curpoint;
global segR;
%-----------------------
%------初始化种群的个体(可以在这里限定位置和速度的范围)------------

for i=1:N
    x(i,:) = initX();     %初始化x
    for j=1:m
        v(i,j) = rand(1)*2*pi;
    end
end

%------先计算各个粒子的适应度，并初始化Pi和Pg----------------------

for i=1:N
    p(i)=fitness(x(i,:),m);
    y(i,:)=x(i,:);              %y记录前一次迭代，粒子的位置
end
pg=x(1,:);             %Pg为全局最优
for i=2:N
    if fitness(x(i,:),m)<fitness(pg,m)
        pg=x(i,:);
    end
end

%------进入主要循环，按照公式依次迭代，直到满足精度要求------------

for t=1:MaxDT
    for i=1:N
        %----cjss更新x
        for q = 1:Qmax
            for j = 1:m
                for k=1:Kmax
                    %%%粒子群更新公式%%%%%%%%%%%
                    v(i,j)=w*v(i,j)+c1*rand*(y(i,j)-x(i,j))...
                            +c2*rand*(pg(j)-x(i,j));
                    x(i,j)=x(i,j)+v(i,j);
                    %%%%%%%%%%%%%%%%%%%%%%%%

                    %如果j是1的话就将curpoint赋值给purPoint
                    %如果不是的话讲将前一个的tempPoint赋值给purPoint
                    if 1 == j
                        purPoint = curpoint;
                    else
                        purPoint = tempPoint;
                    end

                    %将极坐标转换为直角坐标
                    [tempPoint(1),tempPoint(2)]=plorTozhijiao(j*segR,x(i,j));

                    %判断这个点和前面的点是否相通
                    if Conn(purPoint(1),purPoint(2),...
                            tempPoint(1),tempPoint(2)) == true
                        break;
                    end

                end
            end

            %如果k足够小，说明初始化的x是可以连通的跳出循环,直接返回x
            if k<=Kmax
                break ;
            end
        end

        %---更新x
        % x(i,:)=x(i,:)+v(i,:);

        if fitness(x(i,:),m)<p(i)
            p(i)=fitness(x(i,:),m);
            y(i,:)=x(i,:);
        end
        if p(i)<fitness(pg,m)
            pg=y(i,:);
        end
    end
    Pbest(t)=fitness(pg,m);
end

% %------最后给出计算结果
% 
% disp('*************************************************************')
% disp('函数的全局最优位置为：')
% Solution=pg'
% disp('最后得到的优化极值为：')
Result=fitness(pg,m)
% disp('*************************************************************')
% 
% %------算法结束---DreamSun GL & HF-----------------------------------