【三维路径规划】基于matlab遗传算法无人机三维路径规划【含Matlab源码 1268期】

一、无人机简介

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

1 常见的航迹规划算法

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

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

二、遗传算法简介

1 引言

2 遗传算法理论
2.1 遗传算法的生物学基础

2.2 遗传算法的理论基础

2.3 遗传算法的基本概念

2.4 标准的遗传算法

2.5 遗传算法的特点

2.6 遗传算法的改进方向

3 遗传算法流程

4 关键参数说明

三、部分源代码

%%%%%%%%%% AUV path planning using GA %%%%%%%%%%
%%%%%Run 'RP_coordinate.m' before running main %%%%clear all;
close all;
tic;%Runtime timer
%% global variablesload ('coor.mat');   %Load data generated by RP_coordinate.mPopsize =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 rateFitconst=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 genreationsfitness = Fitness(Parentpop,adjacency);       %Calculate fitness of parentscrossover = Crossover(Parentpop,Pcross);      %CrossoverChildpop = Mutation(crossover,Pmutation);     %Mutate and get chindrencombopop=[Parentpop;Childpop];                %Combine parents and chindrencombofitness=Fitness(combopop,adjacency);       %Calculate overall fitnessnextpop=Select(combopop,combofitness);        %Select the first half of best to get 2nd genParentpop=nextpop.pop;if(Generation ==1)Best_GApath=Parentpop(1,:);Best_Fitness=combofitness(nextpop.bestplan);elseNew_Best_Fitness=combofitness(nextpop.bestplan);%Evaluate best solutionNew_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%%%%%%%%elseFitconst=Fitconst+1;endendFitnesscurve(Generation)=Best_Fitness;elsebreakendGeneration = Generation +1;endtoc;
%% plot result planGAplancoor = [RP(Best_GApath).x;RP(Best_GApath).y; RP(Best_GApath).z].';figure(1);for i=1:RPNUMsubplot(2,1,1);     %Plot all rendezvous pointsplot3(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;endsubplot(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 fitnessfigure(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 occursPc = unifrnd(0,1);      if(Pc<Pcross)SS = unidrnd(size(crossover,2)); %Start point of crossover sectionSE = unidrnd(size(crossover,2));   %End point of crossover sectionwhile(SS == SE) SE = unidrnd(size(crossover,2));endif(SE<SS) %Ordertemp = SE;SE = SS;SS=temp;endChrom1=crossover(k,:);          %First chromosome for crossoverChrom2=crossover(k+1,:);        %Second chromosome for crossoverCS2=Chrom1(SS:SE);        %crossover section 1CS1=Chrom2(SS:SE);        %crossover section 2Chrom1(SS:SE)=CS1;        %crossover finishedChrom2(SS:SE)=CS2;        %crossover finished%Avoid conflictLIST=unique(Chrom1); %list all unique numbersCOUNTA=hist(Chrom1,unique(Chrom1)); %Distribute elements on chromosomes to corresponding unique numbersISDUP = COUNTA - ones(1,size(COUNTA,2)); %If there is a duplicate number, the result will be non-zero arrayDUPElem=LIST(find(ISDUP));          %Find the duplicate elementsElemPosition=ismember(CS1,DUPElem); %Find the duplicate elements' positionRELATION=zeros(1,size(Chrom1,2));%Set up relacement relation tablei=1;while i<=size(CS1,2)if((ElemPosition(i)==0))i=i+1;elsea=CS1(i);b=CS2(i);if (~ismember(b,CS1))RELATION(a)=b;RELATION(b)=a;elsewhile(ismember(b,CS1))temp=b;position=find(CS1==temp);b=CS2(position); %#ok<FNDSB>endRELATION(a)=b;RELATION(b)=a;endi=i+1;endendj=1;%Replacementwhile(j<=size(Chrom1,2))while(j>=SS&&j<=SE)j=j+1;endif(j>size(Chrom1,2))breakendif(RELATION(Chrom1(j))==0)j=j+1;elseChrom1(j)=RELATION(Chrom1(j));j=j+1;endendj=j-1;while(j~=0)while(j>=SS&&j<=SE)j=j-1;endif(j==0)break;endif(RELATION(Chrom2(j))==0)j=j-1;elseChrom2(j)=RELATION(Chrom2(j));j=j-1;endendcrossover(k,:)=Chrom1;crossover(k+1,:)=Chrom2;endk=k+2;
end

四、运行结果

五、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)