MATLAB遗传算法规划机器人路径（栅格）源码
MATLAB遗传算法规划机器人路径（栅格）源码
- 运行结果如图
- 代码
- 修改于2019-4-2
- 改于2019-4-4-22
#网上路径规划的代码很多，但是简单栅格路径的规划问题，各位大佬都懒得写，小弟为你奉上，敬请食用（此代码是根据大佬的源码改写而成，由于不知大佬链接，所以就不放了，但是大佬是开源的）
主要包括以下几个部分
text1.m主函数
chack.m边界检测函数
calfitvalue.m适用度函数
crossover.m交叉函数
mutation.m变异函数
poppinit.m初始化族群
select.m
best.m
运行结果如图
链接: link.
图片: 
带尺寸的图片:
代码
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%函数名称：主函数text1.m
%%入口参数：无
%%出口参数：无
%%说明：无
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
clear
pc=0.7; %交叉概率
pm=0.006; %变异概率
[x,y]=popinit(500,10); %产生初始种群
D=calfitvalue(x,y); %计算种群适应度
for i=1:1:200 %设置进化代数
[newx,newy]=selection(x,y,D); %选择
[newx,newy]=crossover(newx,newy,pc); %交叉
[newx,newy]=mutation(newx,newy,pm); %变异
D=calfitvalue(newx,newy); %重新计算适应度
[newx,newy]=selection(newx,newy,D); %选择 保证交叉变异后的种群都不经过障碍物
D=calfitvalue(newx,newy); %重新计算适应度 选择之后的种群适应度也已经发生变化。所以必须重新计算
[bestx,besty,bestfit]=best(newx,newy,D); %选择最佳个体
bbestx(i,:)=bestx; %保存最佳个体
bbesty(i,:)=besty;
bbestfit(i)=bestfit;
x=newx;
y=newy;
end
[bbbestfit,I]=max(bbestfit) %计算选择出来的最佳个体中 适应度最大的个体，作为最优值输出
bbbestx=bbestx(I,:);
bbbesty=bbesty(I,:);
%根据适应度函数中建立的环境模型，填充障碍物
% fill([8,12,12,8,8],[8,8,12,12,8],[0,0,0])
% hold on
% fill([2,6,6],[2,2,4],[0,0,0])
% hold on
% fill([4,7,2],[6,15,15],[0,0,0])
% hold on
% fill([9,16,16],[16,10,18],[0,0,0])
% hold on
% fill([18,7,12],[1,6,2],[0,0,0])
% hold on
fill([2,4,4,2,2],[0,0,2,2,0],[0,0,0])
hold on
fill([0,2,2,4,4,0,0],[6,6,4,4,8,8,6],[0,0,0])
hold on
fill([2,6,6,2,2],[12,12,14,14,12],[0,0,0])
hold on
fill([0,2,2,0,0],[18,18,20,20,18],[0,0,0])
hold on
fill([6,10,10,6,6],[2,2,6,6,2],[0,0,0])
hold on
fill([8,10,10,8,8],[16,16,18,18,16],[0,0,0])
hold on
fill([10,12,12,10,10],[12,12,14,14,12],[0,0,0])
hold on
fill([12,14,14,12,12],[6,6,8,8,6],[0,0,0])
hold on
fill([12,14,14,12,12],[18,18,20,20,18],[0,0,0])
hold on
fill([14,18,18,16,16,14,14],[12,12,14,14,16,16,12],[0,0,0])
hold on
fill([16,20,20,16,16],[2,2,4,4,2],[0,0,0])
hold on
fill([18,20,20,18,18],[8,8,10,10,8],[0,0,0])
hold on
%%画出最有路径的路线
plot(bbbestx,bbbesty,'r-')
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%函数名称：边界相角检测函数 chack.m
%%入口参数：两条线段的顶点。例如 判断线段PQ 与 XY 是否相交，其中 P（x1,y1）、Q(x2,y2)、X(x3,y3)、Y(x4,y4)
%%则函数参数设置为：chack(x1,y1,x2,y2,x3,y3,x4,y4)
%%出口参数： 是否相交 0：不相交 1：相交
%%说明：
%%该函数可以检测两条线断是否相交，当入口参数是向量的时候，可以同时检测多条线段是否相交。
%%该函数入口参数可以为向量，向量可以是列向量也可以是行向量，但必须保证所有入口参数的维度相同。
%%检测方式采用相互跨立实验实现。当两条线段相互跨立时，则两线段在二维平面中相交。
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [pop]=chack(x1,y1,x2,y2,x3,y3,x4,y4)
[px,py]=size(x1);
p=max(px,py); %实现行向量列向量都可以进行检测
[px,py]=size(x3);
q=max(px,py);
for j=1:1:q %检测线段相交的次数
for i=1:1:p %检测所有种群相邻点连成的线段是否与当前障碍物边界相交
pabx=x4(j)-x3(j);
paby=y4(j)-y3(j);
pacx=x2(i)-x3(j);
pacy=y2(i)-y3(j);
padx=x1(i)-x3(j);
pady=y1(i)-y3(j);
m=pabx*pacy-paby*pacx;
n=pabx*pady-paby*padx;
pcdx=x1(i)-x2(i);
pcdy=y1(i)-y2(i);
pcax=x3(j)-x2(i);
pcay=y3(j)-y2(i);
pcbx=x4(j)-x2(i);
pcby=y4(j)-y2(i);
mm=pcdx*pcay-pcdy*pcax;
nn=pcdx*pcby-pcdy*pcbx;
if m*n<=0 && mm*nn<=0 %相交
pop(j,i)=1; %将结果存到矩阵[pop]中。
else %显然矩阵[pop]是一个q行p列的矩阵。
pop(j,i)=0;
end
end
end
end
`%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%函数名称：适应度函数 calfitvalue.m
%%入口参数：初始种群
%%出口参数：种群适应度值 一维列向量
%%说明：
%%种群适应度以种群路径大小作为主要参考标准，路径越短，适应度越大。以一个较大的数减去种群路径长度作为种群的适应度。
%%有障碍物的情况下的环境建模，在此处进行。向量x3、y3、x4、y4即为障碍物边界，并对所有种群的相邻点进行干涉测试，如果有交点则直接将其适应度设为0。
%%函数in=inpolygon(x(j,:),y(j,:),xv,yv);
%%为检测种群点是否在障碍物内部，其实有了上面的干涉检测，该部分程序不用也可以。
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function fitvalue=calfitvalue(x,y)
yv=[10,12,12,10,10];
xv=[12,12,14,14,12];
[px,py]=size(x);
% x3=[8 8 12 12 2 6 6 4 7 2 9 16 16 18 7 12]; %设置环境模型
% y3=[8 12 12 8 2 2 4 6 15 15 16 10 18 1 6 2]; %x3、y3、x4、y4 为所有障碍物边界的相邻顶点组成的线段。
% x4=[8 12 12 8 6 6 2 7 2 4 16 16 9 7 12 18];
% y4=[12 12 8 8 2 4 2 15 15 6 10 18 16 6 2 1];
x3=[2,4,4,2,2,4,4,0,0,2,2,6,6,2,0,2,2,0,6,10,10,6,8,10,10,8,12,14,14,12,10,12,12,10,12,14,14,12,14,18,18,16,16,14,16,20,20,16,18,20,20,18];
y3=[0,0,2,2,4,4,8,8,6,6,12,12,14,14,18,18,20,20,2,2,6,6,16,16,18,18,6,6,8,8,12,12,14,14,18,18,20,20,12,12,14,14,16,16,2,2,4,4,8,8,10,10];
x4=[4,4,2,2,4,4,0,0,2,2,6,6,2,2,2,2,0,0,10,10,6,6,10,10,8,8,14,14,12,12,12,12,10,10,14,14,12,12,18,18,16,16,14,14,20,20,16,16,20,20,18,18,];
y4=[0,2,2,0,4,8,8,6,6,4,12,14,14,12,18,20,20,18,2,6,6,2,16,18,18,16,6,8,8,6,12,14,14,12,18,20,20,18,12,14,14,16,16,12,2,4,4,2,8,10,10,8];
% fill([8,12,12,8,8],[8,8,12,12,8],[0,0,0])
% hold on
% fill([2,6,6],[2,2,4],[0,0,0])
% hold on
% fill([4,7,2],[6,15,15],[0,0,0])
% hold on
% fill([9,16,16],[16,10,18],[0,0,0])
% hold on
% fill([18,7,12],[1,6,2],[0,0,0])
% hold on
for j=1:1:px
in=inpolygon(x(j,:),y(j,:),xv,yv); %判断 点 是否在障碍物内
for i=1:1:py-1
x1(i)=x(j,i);
y1(i)=y(j,i);
end
for i=2:1:py
x2(i-1)=x(j,i);
y2(i-1)=y(j,i);
end
ch=chack(x1,y1,x2,y2,x3,y3,x4,y4); %判断种群相邻点连成的线段是否与所有障碍物边界相交
if sum(in)>0 ||sum(sum(ch))>0 %如果矩阵 [ch] 所有元素的和不等于0,则说明存在某一条线段经过障碍物边界。
dd(j)=0;
else
for i=1:1:py-1
d(i)=sqrt((x(j,i+1)-x(j,i)).^2+(y(j,i+1)-y(j,i)).^2); %如果不经过障碍物，则求取该个体的路径长度，最为适应度计算依据
end
dd(j)=200-sum(d); %计算个体适应度
end
end
fitvalue=dd'; %返回列向量
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%函数名称：交叉函数 crossover.m
%%入口参数：初始种群 交叉概率
%%出口参数：新种群
%%说明：
%%通过随机数的方式决定某一种族是否交叉。 rand%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%函数名称：变异函数 mutation.m
%%入口参数：初始种群 变异概率
%%出口参数：新种群
%%说明：
%%通过随机数的方式决定某一种族是否变异。 rand
%%变异完成之后的种群还必须进行从大到小的排序。
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [newx,newy]=mutation(x,y,pm)
[px,py]=size(x);
newx=x;
newy=y;
for i=1:1:px
if(rand之前程序并不完整，需要添加后面两段程序
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%函数名称：选择函数 selection.m
%%说明：
function [newx,newy]=selection(x,y,fitvalue)
totalfit=sum(fitvalue); %求适应值之和
fitvalue1=fitvalue/totalfit; %单个个体被选择的概率
fitvalue=cumsum(fitvalue1); %如 fitvalue=[1 2 3 4]，则 cumsum(fitvalue)=[1 3 6 10]
[px,py]=size(fitvalue);
ms=sort(rand(px,1)); %从小到大排列 生成一个 px行 1列 的随机矩阵，然后从小到大排列
fitin=1;
newin=1;
while newin<=px
if (ms(newin))0 %fitvalue1(fitin)>0 保证适应度为0的个体不被选中
newx(newin,:)=x(fitin,:);
newy(newin,:)=y(fitin,:);
newin=newin+1;
else
fitin=fitin+1;
end
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%函数名称：最有个体选择函数 best.m
%%入口参数：初始种群 种群适应度
%%出口参数：最佳个体 最佳个体适应度
%%说明：
%%按照适应度大小进行选择最佳个体。适应度最大的个体将被选出，作为函数返回值返回。
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [bestx,besty,bestfit]=best(x,y,fitvalue)
[px,py]=size(x);
bestx=x(1,:);
besty=y(1,:);
bestfit=fitvalue(1);
for i=2:px
if fitvalue(i)>bestfit
bestx=x(i,:);
besty=y(i,:);
bestfit=fitvalue(i);
end
end
end
修改于2019-4-2
在之前mutatioin.m末尾添加了
for i=1:1:px
newx(i,:)=sort(newx(i,:));
newy(i,:)=sort(newy(i,:));
end
end
且将原来popinit.m的最后一句话
function [newx,newy]=selection(x,y,fitvalue)删除
再次运行，可以成功
改于2019-4-4-22
目前本人主要在学习点云处理相关方面的东西，由于作者菜鸡，并非MATLAB大神，所以对于遗传算法相关问题确实难以解答，各位可以查阅其他资料。另：此代码只是为了交一次作业而改写，抱歉各位不能回答你们的具体问题。