pso.m

粒子群(PSO)路径规划。这个是一个局部路径规划

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

%psosued = ones(1,N);         %粒子群成功
Xmax = dim*pathta;                    %最大x值
Xmin = (dim-1)*pathta;                    %最小x值
Vmax = (Xmax - Xmin)*0.1;

for i=1:N
    [temp,psosued(i)] = initX(Xmax,Xmin);
     %如果初始化不成功则不做任何的v变化
    if psosued(i)
        x(i,:) = temp;
        for j=1:m
            v(i,j) = -Vmax+2*rand*Vmax;
        end
    end
end

%判断是否有条可走路径
for i=1:N
    if psosued(i)
        break;
    end
end

if psosued(i) == false
    result = false;
    return;
end

%------先计算各个粒子的适应度，并初始化Pi和Pg----------------------
%如果最大的
best_fitness = 10000;

for i=1:N
    %如果粒子群初始化不成功的话将其适应值改为10000
    if psosued(i)
        p(i)=fitness(x(i,:),m);
        y(i,:)=x(i,:);  %y记录前一次迭代，粒子的位置
        if p(i)<best_fitness
            pg = x(i,:);
            best_fitness = p(i);
        end
    end
end


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

for t=1:MaxDT

    for i=1:N
        %如果psosued为false的话，就让这个粒子不迭代
        if psosued(i)
            %----cjss更新x
            for q = 1:Qmax
                %保护现场如果不通，直接返回
                yuanx = x(i,:);
                yuanv = v(i,:);
                for j = 1:m
                    for k=1:Kmax
                        %保护现场，如果更新后不连通，返回原来的x，v值
                        tempv = v(i,j);
                        tempx = x(i,j);
                        %%%粒子群更新公式%%%%%%%%%%%
                        v(i,j)=w*v(i,j)+c1*rand*(y(i,j)-x(i,j))...
                            +c2*rand*(pg(j)-x(i,j));
                        %看v是否越界
                        if v(i,j)>Vmax
                            v(i,j) = Vmax;
                        else if v(i,j)<-Vmax;
                                v(i,j) = -Vmax;
                            end
                        end

                        x(i,j)=x(i,j)+v(i,j);
                        %看x是否越界
                        if x(i,j)>Xmax
                            x(i,j) = Xmax;
                        else if x(i,j)<Xmin
                                x(i,j) = Xmin;
                            end
                        end
                        %%%%%%%%%%%%%%%%%%%%%%%%

                        %如果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));

                        %判断这个点和前面的点是否相通
                        r = Conn(purPoint(1),purPoint(2),tempPoint(1),tempPoint(2));
                        if r
                            break;
                        else
                            v(i,j) = tempv;
                            x(i,j) = tempx;
                        end
                    end
                    %如果k太大的话，说明此路不通，直接重新开始
                    if r == false
                        break ;
                    end
                end
                if r
                    break;
                else
                    %保护现场如果不通，直接返回
                    x(i,:)=yuanx;
                    v(i,:)=yuanv;
                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
    end
    %记录每次迭代的最好fitness
    Pbest(t)=fitness(pg,m);
end

%----------更新链表
%将这一维的字赋值维2,然后初始化下一个节点
pamoveta(curstep,dim) = 2;%2表示已经走过了
curstep = curstep+1;%将走的点赋值给当前值，继续搜索
pamoveta(curstep,:)= 1;
%将下一段的维值
tempta = mod(numta/2+dim,numta);
if  tempta == 0
    tempta = 6;
end
pamoveta(curstep,tempta) = 3;%3表示来的路
%------------
%------更新点-------------
for j=1:m
    [tempPoint(1),tempPoint(2)]=plorTozhijiao(j*segR,pg(j));
    pointIndex = pointIndex+1;
    pathpoint(pointIndex,1) = tempPoint(1);       %路径规划得出的点
    pathpoint(pointIndex,2) = tempPoint(2);
end
pathpoint
curpoint(1) = tempPoint(1);
curpoint(2) = tempPoint(2);
hadsteps(curstep,1) = curpoint(1);
hadsteps(curstep,2) = curpoint(2);

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