【三维路径规划】遗传算法无人机三维路径规划【Matlab 1268期】

一、无人机简介

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

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

二、遗传算法简介

1 引言
【三维路径规划】遗传算法无人机三维路径规划【Matlab 1268期】
【三维路径规划】遗传算法无人机三维路径规划【Matlab 1268期】
2 遗传算法理论
2.1 遗传算法的生物学基础
【三维路径规划】遗传算法无人机三维路径规划【Matlab 1268期】
【三维路径规划】遗传算法无人机三维路径规划【Matlab 1268期】
2.2 遗传算法的理论基础
【三维路径规划】遗传算法无人机三维路径规划【Matlab 1268期】
【三维路径规划】遗传算法无人机三维路径规划【Matlab 1268期】
【三维路径规划】遗传算法无人机三维路径规划【Matlab 1268期】
【三维路径规划】遗传算法无人机三维路径规划【Matlab 1268期】
2.3 遗传算法的基本概念
【三维路径规划】遗传算法无人机三维路径规划【Matlab 1268期】
【三维路径规划】遗传算法无人机三维路径规划【Matlab 1268期】
【三维路径规划】遗传算法无人机三维路径规划【Matlab 1268期】
【三维路径规划】遗传算法无人机三维路径规划【Matlab 1268期】
【三维路径规划】遗传算法无人机三维路径规划【Matlab 1268期】
【三维路径规划】遗传算法无人机三维路径规划【Matlab 1268期】
2.4 标准的遗传算法
【三维路径规划】遗传算法无人机三维路径规划【Matlab 1268期】
【三维路径规划】遗传算法无人机三维路径规划【Matlab 1268期】
2.5 遗传算法的特点
【三维路径规划】遗传算法无人机三维路径规划【Matlab 1268期】
【三维路径规划】遗传算法无人机三维路径规划【Matlab 1268期】
2.6 遗传算法的改进方向
【三维路径规划】遗传算法无人机三维路径规划【Matlab 1268期】
3 遗传算法流程
【三维路径规划】遗传算法无人机三维路径规划【Matlab 1268期】
【三维路径规划】遗传算法无人机三维路径规划【Matlab 1268期】
【三维路径规划】遗传算法无人机三维路径规划【Matlab 1268期】
4 关键参数说明
【三维路径规划】遗传算法无人机三维路径规划【Matlab 1268期】

三、部分源代码

%%%%%%%%%% AUV path planning using GA %%%%%%%%%%
%%%%%Run 'RP_coordinate.m' before running main %%%%

clear all;
close all;
tic;%Runtime timer
%% global variables

load ('coor.mat');   %Load data generated by RP_coordinate.m

Popsize =50;         %Population size, should be an even integer

%Genetic parameters
%MIXRATE = 0.3;
ITERATION = 10000;   %Number of iteration
THRESHOLD = 100;
Pcross = 0.7;       %Crossover rate
Pmutation = 0.3;    %Mutation rate


Fitconst=0;         %Number of generations that fitness values remain constant
%% Genetic algorithm
while(Generation <= ITERATION)
    if (Fitconst<=THRESHOLD) %Stop iteration if fitness value is constant in threshold number of genreations
        fitness = Fitness(Parentpop,adjacency);       %Calculate fitness of parents
        crossover = Crossover(Parentpop,Pcross);      %Crossover
        Childpop = Mutation(crossover,Pmutation);     %Mutate and get chindren
        combopop=[Parentpop;Childpop];                %Combine parents and chindren
        combofitness=Fitness(combopop,adjacency);       %Calculate overall fitness
        nextpop=Select(combopop,combofitness);        %Select the first half of best to get 2nd gen
        Parentpop=nextpop.pop;
        if(Generation ==1)
            Best_GApath=Parentpop(1,:);
            Best_Fitness=combofitness(nextpop.bestplan);
        else
            New_Best_Fitness=combofitness(nextpop.bestplan);%Evaluate best solution
            New_Best_GApath=Parentpop(1,:);
            
   
                
                %%%%%%%%Visualize planning process%%%%%%%%
%                     GENERATION=[1:Generation-1];
%                     GAplancoor = [RP(Best_GApath).x;RP(Best_GApath).y; RP(Best_GApath).z].';
%                     figure(1);
%                     for i=1:RPNUM
%                         subplot(2,1,1);     %Plot all rendezvous points
%                         plot3(RP(i).x,RP(i).y,RP(i).z,'o');
%                         text(RP(i).x,RP(i).y, RP(i).z,num2str(i));
%                         hold on;
%                         subplot(2,1,2);
%                         plot(RP(i).x,RP(i).y,'o');
%                         text(RP(i).x,RP(i).y,num2str(i));
%                         hold on;
%                     end
%                     subplot(2,1,1);
%                     plot3(GAplancoor(:,1),GAplancoor(:,2),GAplancoor(:,3),'r-.');
%                     title('3D Path of AUV');
%                     grid on;
%                     hold off;
%                     subplot(2,1,2);
%                     plot(GAplancoor(:,1),GAplancoor(:,2),'r-.');
%                     title('2D Path of AUV');
%                     grid on;
%                     hold off;
                %%%%%%%%Visualize planning process%%%%%%%%
                
            else
                Fitconst=Fitconst+1;
            end
        end
        
        Fitnesscurve(Generation)=Best_Fitness;
    
    else
        
        break
        
    end
    
    Generation = Generation +1;
    
end


toc;
%% plot result plan
 GAplancoor = [RP(Best_GApath).x;RP(Best_GApath).y; RP(Best_GApath).z].';
        figure(1);
        for i=1:RPNUM
        subplot(2,1,1);     %Plot all rendezvous points
        plot3(RP(i).x,RP(i).y,RP(i).z,'o');
        text(RP(i).x,RP(i).y, RP(i).z,num2str(i));
        hold on;
        subplot(2,1,2);
        plot(RP(i).x,RP(i).y,'o');
        text(RP(i).x,RP(i).y,num2str(i));
        hold on;
        end
        subplot(2,1,1);
        plot3(GAplancoor(:,1),GAplancoor(:,2),GAplancoor(:,3),'r-.');
        title('3D Path of AUV');
        grid on;
        subplot(2,1,2);
        plot(GAplancoor(:,1),GAplancoor(:,2),'r-.');
        title('2D Path of AUV');
        grid on;
 %% Plot iteration of fitness
 figure(2);
 plot(GENERATION,Fitnesscurve,'r.-');
 title('Minimum distance in each generation');
 xlabel('Generation');
 ylabel('Fitness value');
 legend('Best Fitness Value');
 set(gca, 'Fontname', 'Times New Roman', 'FontSize', 14);
 grid on;
 
% Function for crossover and avoiding conflicts
function crossover = Crossover(pop,Pcross)
crossover=pop;
k=1;
while (k<=(size(crossover,1)-1))
    %Russian roulette to decide whether crossover occurs
    Pc = unifrnd(0,1);      
    if(Pc<Pcross)
        SS = unidrnd(size(crossover,2)); %Start point of crossover section
        SE = unidrnd(size(crossover,2));   %End point of crossover section
        while(SS == SE) 
            SE = unidrnd(size(crossover,2));
        end
        if(SE<SS) %Order
            temp = SE;
            SE = SS;
            SS=temp;
        end
        Chrom1=crossover(k,:);          %First chromosome for crossover
        Chrom2=crossover(k+1,:);        %Second chromosome for crossover
        CS2=Chrom1(SS:SE);        %crossover section 1
        CS1=Chrom2(SS:SE);        %crossover section 2
        Chrom1(SS:SE)=CS1;        %crossover finished
        Chrom2(SS:SE)=CS2;        %crossover finished
        
        %Avoid conflict
        LIST=unique(Chrom1); %list all unique numbers
        COUNTA=hist(Chrom1,unique(Chrom1)); %Distribute elements on chromosomes to corresponding unique numbers
        ISDUP = COUNTA - ones(1,size(COUNTA,2)); %If there is a duplicate number, the result will be non-zero array
        DUPElem=LIST(find(ISDUP));          %Find the duplicate elements
        ElemPosition=ismember(CS1,DUPElem); %Find the duplicate elements' position
        RELATION=zeros(1,size(Chrom1,2));
        
        %Set up relacement relation table
        i=1;
        while i<=size(CS1,2)
            if((ElemPosition(i)==0))
                i=i+1;
            else
                a=CS1(i);
                b=CS2(i);
                if (~ismember(b,CS1))
                    RELATION(a)=b;
                    RELATION(b)=a;
                else
                    while(ismember(b,CS1))
                        temp=b;
                        position=find(CS1==temp);
                        b=CS2(position); %#ok<FNDSB>
                    end
                    RELATION(a)=b;
                    RELATION(b)=a;
                end
                i=i+1;
            end
        end
        j=1;
        %Replacement
        while(j<=size(Chrom1,2))
            while(j>=SS&&j<=SE)
                j=j+1;
            end
            if(j>size(Chrom1,2))
                break
            end
            if(RELATION(Chrom1(j))==0)
                j=j+1;
            else
                Chrom1(j)=RELATION(Chrom1(j));
                j=j+1;
            end
        end
        j=j-1;
        while(j~=0)
            while(j>=SS&&j<=SE)
                j=j-1;
            end
            if(j==0)
                break;
            end
            if(RELATION(Chrom2(j))==0)
                j=j-1;
            else
                Chrom2(j)=RELATION(Chrom2(j));
                j=j-1;
            end
        end
        
        crossover(k,:)=Chrom1;
        crossover(k+1,:)=Chrom2;
        
    end
    k=k+2;
end

四、运行结果

【三维路径规划】遗传算法无人机三维路径规划【Matlab 1268期】
【三维路径规划】遗传算法无人机三维路径规划【Matlab 1268期】

五、matlab版本及参考文献

1 matlab版本
2014a

2 参考文献
[1] 包子阳,余继周,杨杉.智能优化算法及其MATLAB实例(第2版)[M].电子工业出版社,2016.
[2]张岩,吴水根.MATLAB优化算法源代码[M].清华大学出版社,2017.
[3]巫茜,罗金彪,顾晓群,曾青.基于改进PSO的无人机三维航迹规划优化算法[J].兵器装备工程学报. 2021,42(08)
[4]邓叶,姜香菊.基于改进人工势场法的四旋翼无人机航迹规划算法[J].传感器与微系统. 2021,40(07)
[5]马云红,张恒,齐乐融,贺建良.基于改进A*算法的三维无人机路径规划[J].电光与控制. 2019,26(10)
[6]焦阳.基于改进蚁群算法的无人机三维路径规划研究[J].舰船电子工程. 2019,39(03)

上一篇:1268. Search Suggestions System (M)


下一篇:LED灯欧盟erp能效报告eu2019