matlab机器人路径 栅格,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大神，所以对于遗传算法相关问题确实难以解答，各位可以查阅其他资料。另：此代码只是为了交一次作业而改写，抱歉各位不能回答你们的具体问题。