三维路径规划基于matlab改进的蝙蝠算法农用无人机三维路径规划含Matlab源码 1514期

Posted 2021-12-10 紫极神光

一、无人机简介

0 引言
随着现代技术的发展，飞行器种类不断变多，应用也日趋专一化、完善化，如专门用作植保的大疆PS-X625无人机，用作街景拍摄与监控巡察的宝鸡行翼航空科技的X8无人机，以及用作水下救援的白鲨MIX水下无人机等，决定飞行器性能主要是内部的飞控系统和外部的路径规划问题。就路径问题而言，在具体实施任务时仅靠操作员手中的遥控器控制无人飞行器执行相应的工作，可能会对操作员心理以及技术提出极高的要求，为了避免个人操作失误，进而造成飞行器损坏的危险，一种解决问题的方法就是对飞行器进行航迹规划。
飞行器的测量精度，航迹路径的合理规划，飞行器工作时的稳定性、安全性等这些变化对飞行器的综合控制系统要求越来越高。无人机航路规划是为了保证无人机完成特定的飞行任务，并且能够在完成任务的过程中躲避各种障碍、威胁区域而设计出最优航迹路线的问题。

1 常见的航迹规划算法

图1 常见路径规划算法
文中主要对无人机巡航阶段的航迹规划进行研究，假设无人机在飞行中维持高度与速度不变，那么航迹规划成为一个二维平面的规划问题。在航迹规划算法中,A算法计算简单，容易实现。在改进A算法基础上，提出一种新的、易于理解的改进A算法的无人机航迹规划方法。传统A算法将规划区域栅格化，节点扩展只限于栅格线的交叉点，在栅格线的交叉点与交叉点之间往往存在一定角度的两个运动方向。将存在角度的两段路径无限放大、细化，然后分别用两段上的相应路径规划点作为切点，找到相对应的组成内切圆的圆心，然后作弧，并求出相对应的两切点之间的弧所对应的圆心角，根据下式计算出弧线的长度

式中：R———内切圆的半径；
α———切点之间弧线对应的圆心角。

二、蝙蝠优化算法（BA）简介

蝙蝠算法( BA) 是 Yang 教授于 2010 年基于群体智能提出的启发式搜索算法,是一种搜索全局最优解的有效方法。该算法是一种基于迭代的优化技术,初始化为一组随机解,然后 通过迭代搜寻最优解,且在最优解周围通过随机飞行产生局部新解,加强了局部搜索。与其他算法相比,BA 在准确性和有效性方面远优于其他算法,且没有许多参数要进行调整。

三、部分源代码

clear all clc
%改进的蝙蝠算法在无人机航向上的规划应用第X次实验
        startX=0;startY=0;                            %起开始坐标                          
        endX=700;endY=700;                            %结束坐标
        gridCount=30;                                 %段点

%% 蝙蝠算法相关参数
        Qmax=0.5;                                     %最大频率
        Qmin=0;                                       %最小频率
        Rmax=1;
        pop=20;                                       %种群个数
        N_gen=50;                                     %迭代的次数
        c1=2;                                         %粒子群算法系数
        c2=2;                                         %粒子群算法系数
        V=zeros(pop,2*gridCount);                     %速度的初始化
        Q=zeros(pop,gridCount);                       %频率的初始化
        S=zeros(pop,2*gridCount);                     %速度的初始化
        fang=0;                                       %早熟因子
        Std=0;                                        %标准系数
        F0=0.5;                                       %变异因子
        CR=0.5;                                       %杂交参数
        w=0.8;                                        %惯性权重
        pathMax=700;                                  %边界最大值
        pathMin=0;                                    %边界最小值
        a=0.9;
        path_bar_best=[];
        position_bar_best=[];
        %%%%%%%%%%%%%%%%%%%%初始化确立%%%%%%%%%%%%%%%%%%%        
        for i=1:pop           
            f(i)=rand();
            fang=fang+f(i);
            for j=1:2*gridCount
                R(i,j)=rand();
                A(i,j)=rand();
            end
        end
        %%%%%%%%%%%%%%%%%%%%参考差分进化算法中对早熟机制的处理%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        fang=fang/pop;
        for i=1:pop
             f1(i)=abs(f(i)-fang);
        end
        if max(f1(i))>1
            ff=max(f1(i));
        else
            ff=1;
        end
        Std=Std+((f(i)-fang)/ff);
        Std=Std/pop;
%% 生成山峰
          threat=[304 400 0;404 320 0;440 500 0;279 310 0;560 220 0;172 527 0;....
                194 220 0;272 522 0;350 200 0;....
                 650 400 0;740 250 0;540 375 0;510 600 0];
            r=[45 50 55 10 70 65 55 25 50 30 40 40 35];

        for i=1:length(r)
              figure(1)
              [x,y,z]=sphere;
              mesh(threat(i,1)+r(i)*x,threat(i,2)+r(i)*y,abs(threat(i,3)+r(i)*z));
              hold on
        end
        view([-30,-30,70])
%% 初始化粒子数
% for o=1:3
%     switch o
%         case 1
%             N_gen=100;
%         case 2
%             N_gen=200;
%         case 3
%             N_gen=300;
%     end
    tic
%     for u=1:25
                    for i=1:pop
                        for j=1:gridCount
                                X(i,j)=startX+j*(endX-startX)/(gridCount+1);
                                Y(i,j)=startY+rand()*(endY-startY);
                                path(i,2*j-1)=X(i,j);
                                path(i,2*j)=Y(i,j);
                        end
                    end
                    for i=1:pop
                         [distance,pathpoint,positionPoint]=verify(path(i,:),threat,....
                                         r,startX,startY,endX,endY,gridCount);
                        fitness(i)=distance;
                    end
                    [bestFitness,bestindex]=min(fitness);
                    bestpath=path(bestindex,:);
                    T=std(fitness); 
                    BestFitness=Inf;
                    globalFitness=Inf;
                    pathRecord=zeros(1,gridCount+1); bestRecord=zeros(1,gridCount+1);
                    position=zeros(gridCount+1,2);
            %%  迭代开始
                    for t=1:N_gen
                        for i=1:pop    
                          Q(i)=Qmin+(Qmin-Qmax)*rand();                      %蝙蝠算法的核心公式
                          V(i,:)=V(i,:)+(bestpath-path(i,:))*Q(i);
                          S(i,:)=path(i,:)+V(i,:);
                          S(i,find(S(i,:)>pathMax))=pathMax;     
                          S(i,find(S(i,:)<pathMin))=pathMin;
                          aa=6^(-30*(-t/N_gen)^4);
                          if  rand>R(i,:)                                  %step3 进行随机扰动
                               A1=mean(A(i,:));
                               S(i,:)=bestpath(:)+A1*aa;
                          end
                          [distance,pathpoint,positionPoint]=verify(bestpath,threat,....
                                 r,startX,startY,endX,endY,gridCount);
                          fnew=distance;
                          if rand<A(i)&&fitness(i)<fnew;
                              A(i)=a*A(i);
                              R(i,:)=Rmax*(1-exp(-0.9*t));
                          end
                             a=1;b=pop;
                             dx=randperm(b-a+1)+a-1;
                             j=dx(1);k=dx(2);p=dx(3);
                          if j==i
                             j=dx(4);
                          elseif k==i
                             k=dx(4);
                         elseif p==i
                             p=dx(4);
                          end

四、运行结果

五、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 包子阳,余继周,杨杉.智能优化算法及其MATLAB实例（第2版）[M].电子工业出版社，2016.
[2]张岩,吴水根.MATLAB优化算法源代码[M].清华大学出版社，2017.