基于遗传算法的机器人路径规划MATLAB源码

  • 格式:doc
  • 大小:35.50 KB
  • 文档页数:8

基于遗传算法的机器人路径规划MATLAB源码算法的思路如下:取各障碍物顶点连线的中点为路径点,相互连接各路径点,将机器人移动的起点和终点限制在各路径点上,利用Dijkstra算法来求网络图的最短路径,找到从起点P1到终点Pn的最短路径,由于上述算法使用了连接线中点的条件,不是整个规划空间的最优路径,然后利用遗传算法对找到的最短路径各个路径点Pi (i=1,2,…n)调整,让各路径点在相应障碍物端点连线上滑动,利用Pi= Pi1+ti×(Pi2-Pi1)(ti∈[0,1] i=1,2,…n)即可确定相应的Pi,即为新的路径点,连接此路径点为最优路径。

function [L1,XY1,L2,XY2]=JQRLJGH(XX,YY)%% 基于Dijkstra和遗传算法的机器人路径规划演示程序% GreenSim团队原创作品,转载请注明% GreenSim团队长期从事算法设计、代写程序等业务% 欢迎访问GreenSim——算法仿真团队→/greensim%输入参数在函数体内部定义%输出参数为% L1 由Dijkstra算法得出的最短路径长度% XY1 由Dijkstra算法得出的最短路径经过节点的坐标% L2 由遗传算法得出的最短路径长度% XY2 由遗传算法得出的最短路径经过节点的坐标%程序输出的图片有% Fig1 环境地图(包括:边界、障碍物、障碍物顶点之间的连线、Dijkstra的网络图结构)% Fig2 由Dijkstra算法得到的最短路径% Fig3 由遗传算法得到的最短路径% Fig4 遗传算法的收敛曲线(迄今为止找到的最优解、种群平均适应值)%% 画Fig1figure(1);PlotGraph;title('地形图及网络拓扑结构')PD=inf*ones(26,26);for i=1:26for j=1:26if D(i,j)==1x1=XY(i,5);y1=XY(i,6);x2=XY(j,5);y2=XY(j,6);dist=((x1-x2)^2+(y1-y2)^2)^0.5;PD(i,j)=dist;endendend%% 调用最短路算法求最短路s=1;%出发点t=26;%目标点[L,R]=ZuiDuanLu(PD,s,t);L1=L(end);XY1=XY(R,5:6);%% 绘制由最短路算法得到的最短路径figure(2);PlotGraph;hold onfor i=1:(length(R)-1)x1=XY1(i,1);y1=XY1(i,2);x2=XY1(i+1,1);y2=XY1(i+1,2);plot([x1,x2],[y1,y2],'k');hold onendtitle('由Dijkstra算法得到的初始路径')%% 使用遗传算法进一步寻找最短路%第一步:变量初始化M=50;%进化代数设置N=20;%种群规模设置Pm=0.3;%变异概率设置LC1=zeros(1,M);LC2=zeros(1,M);Yp=L1;%第二步:随机产生初始种群X1=XY(R,1);Y1=XY(R,2);X2=XY(R,3);Y2=XY(R,4);for i=1:Nfarm{i}=rand(1,aaa);end% 以下是进化迭代过程counter=0;%设置迭代计数器while counter<M%停止条件为达到最大迭代次数%% 第三步:交叉%交叉采用双亲双子单点交叉newfarm=cell(1,2*N);%用于存储子代的细胞结构Ser=randperm(N);%两两随机配对的配对表A=farm{Ser(1)};%取出父代AB=farm{Ser(2)};%取出父代BP0=unidrnd(aaa-1);%随机选择交叉点a=[A(:,1:P0),B(:,(P0+1):end)];%产生子代ab=[B(:,1:P0),A(:,(P0+1):end)];%产生子代bnewfarm{2*N-1}=a;%加入子代种群newfarm{2*N}=b;for i=1:(N-1)A=farm{Ser(i)};B=farm{Ser(i+1)};newfarm{2*i}=b;endFARM=[farm,newfarm];%新旧种群合并%% 第四步:选择复制SER=randperm(2*N);FITNESS=zeros(1,2*N);fitness=zeros(1,N);for i=1:(2*N)PP=FARM{i};FITNESS(i)=MinFun(PP,X1,X2,Y1,Y2);%调用目标函数endfor i=1:Nf1=FITNESS(SER(2*i-1));f2=FITNESS(SER(2*i));if f1<=f2elsefarm{i}=FARM{SER(2*i)};fitness(i)=FITNESS(SER(2*i));endend%记录最佳个体和收敛曲线minfitness=min(fitness);meanfitness=mean(fitness);if minfitness<Yppos=find(fitness==minfitness);Xp=farm{pos(1)};Yp=minfitness;endif counter==10PPP=[0.5,Xp,0.5]';PPPP=1-PPP;X=PPP.*X1+PPPP.*X2;Y=PPP.*Y1+PPPP.*Y2;XY2=[X,Y];figure(3)PlotGraph;hold onfor i=1:(length(R)-1)x1=XY2(i,1);y1=XY2(i,2);x2=XY2(i+1,1);y2=XY2(i+1,2);plot([x1,x2],[y1,y2],'k');hold onendtitle('遗传算法第10代')hold onfor i=1:(length(R)-1)x1=XY1(i,1);y1=XY1(i,2);x2=XY1(i+1,1);y2=XY1(i+1,2);plot([x1,x2],[y1,y2],'k','LineWidth',1);hold onendendif counter==20PPP=[0.5,Xp,0.5]';PPPP=1-PPP;X=PPP.*X1+PPPP.*X2;Y=PPP.*Y1+PPPP.*Y2;XY2=[X,Y];figure(4)PlotGraph;hold onfor i=1:(length(R)-1)x1=XY2(i,1);y2=XY2(i+1,2);plot([x1,x2],[y1,y2],'k');hold onendtitle('遗传算法第20代')hold onfor i=1:(length(R)-1)x1=XY1(i,1);y1=XY1(i,2);x2=XY1(i+1,1);y2=XY1(i+1,2);plot([x1,x2],[y1,y2],'k','LineWidth',1);hold onendendif counter==30PPP=[0.5,Xp,0.5]';PPPP=1-PPP;X=PPP.*X1+PPPP.*X2;Y=PPP.*Y1+PPPP.*Y2;XY2=[X,Y];figure(5)PlotGraph;hold onfor i=1:(length(R)-1)x1=XY2(i,1);y1=XY2(i,2);x2=XY2(i+1,1);y2=XY2(i+1,2);plot([x1,x2],[y1,y2],'k');hold onendtitle('遗传算法第30代')hold onfor i=1:(length(R)-1)x1=XY1(i,1);y2=XY1(i+1,2);plot([x1,x2],[y1,y2],'k','LineWidth',1);hold onendendif counter==40PPP=[0.5,Xp,0.5]';PPPP=1-PPP;X=PPP.*X1+PPPP.*X2;Y=PPP.*Y1+PPPP.*Y2;XY2=[X,Y];figure(6)PlotGraph;hold onfor i=1:(length(R)-1)x1=XY2(i,1);y1=XY2(i,2);x2=XY2(i+1,1);y2=XY2(i+1,2);plot([x1,x2],[y1,y2],'k');hold onendtitle('遗传算法第40代')hold onfor i=1:(length(R)-1)x1=XY1(i,1);y1=XY1(i,2);x2=XY1(i+1,1);y2=XY1(i+1,2);plot([x1,x2],[y1,y2],'k','LineWidth',1);hold onendendif counter==50PPP=[0.5,Xp,0.5]';PPPP=1-PPP;X=PPP.*X1+PPPP.*X2;Y=PPP.*Y1+PPPP.*Y2;XY2=[X,Y];figure(7)PlotGraph;hold onfor i=1:(length(R)-1)x1=XY2(i,1);y1=XY2(i,2);x2=XY2(i+1,1);y2=XY2(i+1,2);plot([x1,x2],[y1,y2],'k');hold onendtitle('遗传算法第50代')hold onfor i=1:(length(R)-1)x1=XY1(i,1);y1=XY1(i,2);x2=XY1(i+1,1);y2=XY1(i+1,2);plot([x1,x2],[y1,y2],'k','LineWidth',1);hold onendendLC2(counter+1)=Yp;LC1(counter+1)=meanfitness;%% 第五步:变异for i=1:Nif Pm>rand&&pos(1)~=iAA=farm{i};AA(POS)=rand;farm{i}=AA;endendcounter=counter+1;disp(counter);end%% 输出遗传算法的优化结果PPP=[0.5,Xp,0.5]';PPPP=1-PPP;X=PPP.*X1+PPPP.*X2;Y=PPP.*Y1+PPPP.*Y2;XY2=[X,Y];L2=Yp;%% 绘制Fig3figure(8)PlotGraph;hold onhold onfor i=1:(length(R)-1)x1=XY1(i,1);y1=XY1(i,2);x2=XY1(i+1,1);y2=XY1(i+1,2);plot([x1,x2],[y1,y2],'k','LineWidth',1);hold onendfor i=1:(length(R)-1)x1=XY2(i,1);y1=XY2(i,2);x2=XY2(i+1,1);y2=XY2(i+1,2);plot([x1,x2],[y1,y2],'k');hold onendtitle('遗传算法最终结果')figure(9)PlotGraph;hold onfor i=1:(length(R)-1)x1=XY1(i,1);y1=XY1(i,2);x2=XY1(i+1,1);y2=XY1(i+1,2);plot([x1,x2],[y1,y2],'k','LineWidth',1);hold onendhold onfor i=1:(length(R)-1)x1=XY2(i,1);y1=XY2(i,2);x2=XY2(i+1,1);y2=XY2(i+1,2);plot([x1,x2],[y1,y2],'k','LineWidth',2);hold onendtitle('遗传算法优化前后结果比较')%% 绘制Fig4figure(10);plot(LC1);hold onplot(LC2);xlabel('迭代次数');title('收敛曲线');。