【PSO三维路径规划】基于matlab粒子群算法无人机三维路径规划【含Matlab源码 192期】

无人机航迹规划是指在综合考虑无人机飞行油耗、威胁、飞行区域和自身物理条件限制的前提下， 为飞机在飞行区域内从初始点到目标点规划最佳或满意的飞行航迹， 其本质是多约束的目标优化。航迹规划算法是航迹规划的核心。相关研究在国内外相继进行， 提出了许多航迹规划算法， 如模拟退火算法、人工势场法、遗传算法、蚁群算法等。但由于无人机面临的规划空间极其复杂，规划约束多，模糊性大， 搜索算法存在寻优能力差、计算量大、效率低等问题， 航迹规划的最优性和实时性有待进一步提高。

粒子群优化算法 (particle swarm optimization, PSO)是Kennedy和Eberhart群体智能仿生算法于1995年提出， 在解决一些典型的函数优化问题时， 能够取得比较好的优化结果。

1 无人机航迹规划模型
1.1 航迹表示方法
一般地, 无人机航迹规划的空间可以表示为三维坐标系下所有点的集合{ (x, y, z) |xmin≤x≤xmax, ymin≤y≤ymax, zmin≤z≤zmax}, 其中x, y该节点在飞行水平面下的坐标， 这一点的经纬度也可以表示， z是高程数据或海拔高度。航迹规划的目的是在空间中获得无人机的飞行轨迹， 生成的航迹可以表示为三维空间的一系列点{PS, P1, P2, …, Pn-2, PG}, 直线段连接相邻航迹点。

1.2 航迹成本函数
在航迹规划中， 经常使用适当简化的航迹代价计算公式

式中, s表示航迹段数， Li表示第一段航迹的长度， 代表距离成本。Hi表示第一段航迹的平均海拔高度， 代表高成本。Ti是第一段航迹的威胁指数， 代表威胁代价。k1、k2、k三是距离成本、高成本和威胁成本的权重值， 权重的选择与飞行任务的要求有关。

2 基本粒子群算法
粒子群算法初始化为数量为N的随机粒子 (随机解) , 在D维空间中，通过重复迭代和更新自己的位置，搜索适应性值的最佳解。在搜索空间中，粒子的位置代表了题的潜在解决方案。在每次迭代中, 粒子通过跟踪两个极值来更新自己的速度和位置：一是粒子本身目前发现的最佳解， 即个体极值;另一个是整个粒子群目前找到的最优解, 即全局极值。粒子i (i=1, 2, …, N) 在第j (j=1, 2, …, D) 维的速度vij和位置xij按以下格式更新：

式中, ω为非负数, 称为惯性权值 (惯性因子) , 描述了粒子对以前速度的继承， 即反映粒子的惯性；c1和c二是非负常数， 叫学习因素 (加速因子) , 体现了粒子的社会性, 即粒子向全球最优粒子学习的特性；r1和r2为 (0, 1) 随机数之间；pi= (pi1, pi2, …, pi D) 表示粒子i个体极值的位置；pg= (pg1, pg2, …, pg D) 表示所有粒子的整体极值。

第一项速度更新公式， 反映粒子当前速度的影响， 每个粒子按照惯性权值的比例沿自身速度方向搜索， 平衡全局， 第二项反映了个体最优值对粒子速度的影响， 即粒子本身的记忆和认识, 使粒子具有全局搜索能力。第三项反映了群体对个体的影响， 即群体间的信息共享起到加速收敛的作用。

clc clear close all  %% 定义三维路径规划模型 startPos = [40, 129, 5]; goalPos = [951, 833, 10];  % 山峰地图的随机定义 mapRange = [1000,1000,120];              % 地图长、宽、高范围 [X,Y,Z] = defMap4(mapRange);  %% 设置初始参数 N = 100;           % 迭代次数 M = 50;            % 粒子数量 pointNum = 4;      % 每个粒子包含三个位置点 w = 1.2;           % 惯性权重 c1 = 1.5;            % 社会权重 c2 = 1.5;            % 认知权重  % 粒子位置边界 posBound = [[0,0,10]',[1000,1000,60]'];

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

%% 种群初始化
% 初始化一个空的粒子结构体
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,inf];

% 第一代的个体粒子初始化
for i = 1:M 
    % 粒子按照正态分布随机生成
    particles(i).pos.x = unifrnd(posBound(1,1),posBound(1,2),1,pointNum);
    particles(i).pos.x=sort(particles(i).pos.x);
    particles(i).pos.y = unifrnd(posBound(2,1),posBound(2,2),1,pointNum);
    particles(i).pos.y=sort(particles(i).pos.y);
    particles(i).pos.z = unifrnd(posBound(3,1),posBound(3,2),1,pointNum);
    %particles(i).pos.z=sort(particles(i).pos.z);
    
    % 初始化速度
%     particles(i).v.x = zeros(1, pointNum);
%     particles(i).v.y = zeros(1, pointNum);
%     particles(i).v.z = zeros(1, pointNum);
    particles(i).v.x=unifrnd(velBound(1,1),velBound(1,2),1,pointNum);
    particles(i).v.y=unifrnd(velBound(2,1),velBound(2,2),1,pointNum);
    particles(i).v.z=unifrnd(velBound(3,1),velBound(3,2),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(1) < GlobalBest.fitness(1))&(particles(i).Best.fitness(2) < GlobalBest.fitness(2))
        GlobalBest = particles(i).Best;
    end
end

% 初始化每一代的最优适应度，用于画适应度迭代图
fitness_beat_iters = zeros(N,2);

%% 循环
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).fitness = fitness;
            particles(i).path = path;
        end
        
        % 更新个体粒子最优
        if (particles(i).fitness(1) < particles(i).Best.fitness(1))&(particles(i).fitness(2) < particles(i).Best.fitness(2))
            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(1) < GlobalBest.fitness(1))&(particles(i).Best.fitness(2) < GlobalBest.fitness(2))
                GlobalBest = particles(i).Best;
            end
        end
    end
    
    
    % 把每一代的最优粒子赋值给fitness_beat_iters
    fitness_beat_iters(iter,1) = GlobalBest.fitness(1);
    fitness_beat_iters(iter,2) = GlobalBest.fitness(2);
    
    % 在命令行窗口显示每一代的信息
    disp(['第' num2str(iter) '代:' '最优适应度1 = ' num2str(fitness_beat_iters(iter,1))  '最优适应度2 = ' num2str(fitness_beat_iters(iter,2))]);
    
    % 画图
    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(:,1),'LineWidth',2);
xlabel('迭代次数');
ylabel('最优适应度1');

figure
plot(fitness_beat_iters(:,2),'LineWidth',2);
xlabel('迭代次数');
ylabel('最优适应度2');

1 matlab版本
2014a