路徑規劃-粒子群演算法

阿新 • • 發佈：2021-08-07

演算法簡介

演算法與三維路徑規劃結合的思想

演算法流程

程式碼演示：

pso.m

clc
clear
close all

%% 三維路徑規劃模型定義
startPos = [1, 1, 1];
goalPos = [100, 100, 80];

% 隨機定義山峰地圖
mapRange = [100,100,100];              % 地圖長、寬、高範圍
[X,Y,Z] = defMap(mapRange);

%% 初始引數設定
N = 100;           % 迭代次數
M = 50;            % 粒子數量
pointNum = 3;      % 每一個粒子包含三個位置點
w  
= 1.2;           % 慣性權重
c1 = 2;            % 社會權重
c2 = 2;            % 認知權重

% 粒子位置界限
posBound = [[0,0,0]',mapRange'];

% 粒子速度界限
alpha = 0.1;
velBound(:,2) = alpha*(posBound(:,2) - posBound(:,1));
velBound(:,1) = -velBound(:,2);

%% 種群初始化
% 初始化一個空的粒子結構體
particles.pos= [];
particles.v = [];
particles.fitness  
= [];
particles.path = [];
particles.Best.pos = [];
particles.Best.fitness = [];
particles.Best.path = [];

% 定義M個粒子的結構體
particles = repmat(particles,M,1);

% 初始化每一代的最優粒子
GlobalBest.fitness = inf;

% 第一代的個體粒子初始化
for i = 1:M 
    % 粒子按照正態分佈隨機生成
    particles(i).pos.x = unifrnd(posBound(1,1),posBound(1 
,2),1,pointNum);
    particles(i).pos.y = unifrnd(posBound(2,1),posBound(2,2),1,pointNum);
    particles(i).pos.z = unifrnd(posBound(3,1),posBound(3,2),1,pointNum);
    
    % 初始化速度
    particles(i).v.x = zeros(1, pointNum);
    particles(i).v.y = zeros(1, pointNum);
    particles(i).v.z = zeros(1, pointNum);
    
    % 適應度
    [flag,fitness,path] = calFitness(startPos, goalPos,X,Y,Z, particles(i).pos);
    
    % 碰撞檢測判斷
    if flag == 1
        % 若flag=1，表明此路徑將與障礙物相交，則增大適應度值
        particles(i).fitness = 1000*fitness;
        particles(i).path = path;
    else
        % 否則，表明可以選擇此路徑
        particles(i).fitness = fitness;
        particles(i).path = path;
    end
    
    % 更新個體粒子的最優
    particles(i).Best.pos = particles(i).pos;
    particles(i).Best.fitness = particles(i).fitness;
    particles(i).Best.path = particles(i).path;
    
    % 更新全域性最優
    if particles(i).Best.fitness < GlobalBest.fitness
        GlobalBest = particles(i).Best;
    end
end

% 初始化每一代的最優適應度，用於畫適應度迭代圖
fitness_beat_iters = zeros(N,1);

%% 迴圈
for iter = 1:N
    for i = 1:M  
        % 更新速度
        particles(i).v.x = w*particles(i).v.x ...
            + c1*rand([1,pointNum]).*(particles(i).Best.pos.x-particles(i).pos.x) ...
            + c2*rand([1,pointNum]).*(GlobalBest.pos.x-particles(i).pos.x);
        particles(i).v.y = w*particles(i).v.y ...
            + c1*rand([1,pointNum]).*(particles(i).Best.pos.y-particles(i).pos.y) ...
            + c2*rand([1,pointNum]).*(GlobalBest.pos.y-particles(i).pos.y);
        particles(i).v.z = w*particles(i).v.z ...
            + c1*rand([1,pointNum]).*(particles(i).Best.pos.z-particles(i).pos.z) ...
            + c2*rand([1,pointNum]).*(GlobalBest.pos.z-particles(i).pos.z);

        % 判斷是否位於速度界限以內
        particles(i).v.x = min(particles(i).v.x, velBound(1,2));
        particles(i).v.x = max(particles(i).v.x, velBound(1,1));
        particles(i).v.y = min(particles(i).v.y, velBound(2,2));
        particles(i).v.y = max(particles(i).v.y, velBound(2,1));
        particles(i).v.z = min(particles(i).v.z, velBound(3,2));
        particles(i).v.z = max(particles(i).v.z, velBound(3,1));
        
        % 更新粒子位置
        particles(i).pos.x = particles(i).pos.x + particles(i).v.x;
        particles(i).pos.y = particles(i).pos.y + particles(i).v.y;
        particles(i).pos.z = particles(i).pos.z + particles(i).v.z;

        % 判斷是否位於粒子位置界限以內
        particles(i).pos.x = max(particles(i).pos.x, posBound(1,1));
        particles(i).pos.x = min(particles(i).pos.x, posBound(1,2));
        particles(i).pos.y = max(particles(i).pos.y, posBound(2,1));
        particles(i).pos.y = min(particles(i).pos.y, posBound(2,2));
        particles(i).pos.z = max(particles(i).pos.z, posBound(3,1));
        particles(i).pos.z = min(particles(i).pos.z, posBound(3,2));
        
        % 適應度計算
        [flag,fitness,path] = calFitness(startPos, goalPos,X,Y,Z, particles(i).pos);
        
        % 碰撞檢測判斷
        if flag == 1
            % 若flag=1，表明此路徑將與障礙物相交，則增大適應度值
            particles(i).fitness = 1000*fitness;
            particles(i).path = path;
        else
            % 否則，表明可以選擇此路徑
            particles(i).fitness = fitness;
            particles(i).path = path;
        end
        
        % 更新個體粒子最優
        if particles(i).fitness < particles(i).Best.fitness
            particles(i).Best.pos = particles(i).pos;
            particles(i).Best.fitness = particles(i).fitness;
            particles(i).Best.path = particles(i).path;
            
            % 更新全域性最優粒子
            if particles(i).Best.fitness < GlobalBest.fitness
                GlobalBest = particles(i).Best;
            end
        end
    end
    
    
    % 把每一代的最優粒子賦值給fitness_beat_iters
    fitness_beat_iters(iter) = GlobalBest.fitness;
    
    % 在命令列視窗顯示每一代的資訊
    disp(['第' num2str(iter) '代:' '最優適應度 = ' num2str(fitness_beat_iters(iter))]);
    
    % 畫圖
    plotFigure(startPos,goalPos,X,Y,Z,GlobalBest);
    pause(0.001);
end

%% 結果展示
% 理論最小適應度：直線距離
fitness_best = norm(startPos - goalPos);
disp([ '理論最優適應度 = ' num2str(fitness_best)]);

% 畫適應度迭代圖
figure
plot(fitness_beat_iters,'LineWidth',2);
xlabel('迭代次數');
ylabel('最優適應度');

calFitness.m

function [flag,fitness,path] = calFitness(startPos, goalPos,X,Y,Z, pos)
% 利用三次樣條擬合散點
x_seq=[startPos(1), pos.x, goalPos(1)];
y_seq=[startPos(2), pos.y, goalPos(2)];
z_seq=[startPos(3), pos.z, goalPos(3)];

k = length(x_seq);
i_seq = linspace(0,1,k);
I_seq = linspace(0,1,100);
X_seq = spline(i_seq,x_seq,I_seq);
Y_seq = spline(i_seq,y_seq,I_seq);
Z_seq = spline(i_seq,z_seq,I_seq);
path = [X_seq', Y_seq', Z_seq'];

% 判斷生成的曲線是否與與障礙物相交
flag = 0;
for i = 2:size(path,1)
    x = path(i,1);
    y = path(i,2);
    z_interp = interp2(X,Y,Z,x,y);
    if path(i,3) < z_interp
        flag = 1;
        break
    end
end


%% 計算三次樣條得到的離散點的路徑長度（適應度）
dx = diff(X_seq);
dy = diff(Y_seq);
dz = diff(Z_seq);
fitness = sum(sqrt(dx.^2 + dy.^2 + dz.^2));