基于matlab的智能机器人路径规划仿真,机器人路径规划 转自《机器人控制系统的设计与matlab仿真》...

主程序：

clear all;

close all;

t=31; %Number of Cities is t-1

s=500; %Number of Samples

pc=0.90; %交叉概率

pm=0.20; %变异概率

pop=zeros(s,t);

for i=1:s

pop(i,1:t-1)=randperm(t-1);

end

for k=1:1:100 %循环100次

if mod(k,10)==1

k

end

pop=chap10_1dis(pop);

c=15;

pop=chap10_1select(pop,c);

p=rand;

if p>=pc

pop=chap10_1cross(pop); %交叉

end

if p>=pm

pop=chap10_1mutate(pop); %变异

end

end

pop

min(pop(:,t))

J=pop(:,t);

fi=1./J;

[Oderfi,Indexfi]=sort(fi); % Arranging fi small to bigger

BestS=pop(Indexfi(s),:); % Let BestS=E(m), m is the Indexfi belong to max(fi)

I=BestS;

x=[87 91 83 71 64 68 83 87 74 71 58 54 51 37 41 2 7 22 25 18 4 13 18 24 25 41 45 44 58 62];

y=[7 38 46 44 60 58 69 76 78 71 69 62 67 84 94 99 64 60 62 54 50 40 40 42 38 26 21 35 35 32];

%x=[87 58 91 83 62 71 64 68 83 87 74 71 58 54 51 37 41 2 7 22 25 18 4 13 18 24 25 41 45 44];

%y=[7 35 38 46 32 44 60 58 69 76 78 71 69 62 67 84 94 99 64 60 62 54 50 40 40 42 38 26 21 35];

for i=1:1:t-1

x1(i)=x(I(i));

y1(i)=y(I(i));

end

x1(t)=x(I(1));

y1(t)=y(I(1));

figure(1);

plot(x1,y1,'-or');

适应度函数：

function [pop]=qiujuli(pop)

[s,t]=size(pop);

for i=1:1:s

dd=0;

for j=1:1:t-2

dd=dd+chap10_1calculate(pop(i,j),pop(i,j+1));

end

% dd=dd+ga_tsp_juli(pop(i,1),pop(i,t-1));

pop(i,t)=dd; %矩阵最后一行 存储距离的大小

end

距离计算函数：

function [d]=juli(m,n)

x=[87 91 83 71 64 68 83 87 74 71 58 54 51 37 41 2 7 22 25 18 4 13 18 24 25 41 45 44 58 62];

y=[7 38 46 44 60 58 69 76 78 71 69 62 67 84 94 99 64 60 62 54 50 40 40 42 38 26 21 35 35 32];

%%%城市的距离用一对(x,y)表示。

%m=m+1;

%n=n+1;

d=sqrt((x(m)-x(n))^2+(y(m)-y(n))^2);

选择算子函数：

function [pop]=select(pop,k)

[s,t]=size(pop);

m11=(pop(:,t)); %列向量，pop最后一列表示距离远近

%m11

m11=m11'; %m11为1*500行向量

mmax=zeros(1,k); %k=c=15

mmin=zeros(1,k);

num=1;

while num

[a,mmax(num)]=max(m11);

m11(mmax(num))=0;

num=num+1;

end

num=1;

while num

[b,mmin(num)]=min(m11);

m11(mmin(num))=a;

num=num+1;

end

for i=1:k

pop(mmax(i),:)=pop(mmin(i),:); %利用最小值替换最大值

end

交叉算子函数：

function [pop]=cross(pop) %交叉

[s,t]=size(pop);

pop1=pop;

for i=1:2:s

m=randperm(t-3)+1;

crosspoint(1)=min(m(1),m(2));

crosspoint(2)=max(m(1),m(2));

% middle=pop(i,crosspoint(1)+1:crosspoint(2));

% pop(i,crosspoint(1)+1:crosspoint(2))=pop(i+1,crosspoint(1)+1:crosspoint(2));

% pop(i+1,crosspoint(1)+1:crosspoint(2))=middle;

%%%%截取crosspoint(1)前面的部分进行交叉

for j=1:crosspoint(1)

while find(pop(i,crosspoint(1)+1:crosspoint(2))==pop(i,j))

zhi=find(pop(i,crosspoint(1)+1:crosspoint(2))==pop(i,j));

y=pop(i+1,crosspoint(1)+zhi);

pop(i,j)=y;

end

end

%%%%截取crosspoint(2)后面的部分进行交叉

for j=crosspoint(2)+1:t-1

while find(pop(i,crosspoint(1)+1:crosspoint(2))==pop(i,j))

zhi=find(pop(i,crosspoint(1)+1:crosspoint(2))==pop(i,j));

y=pop(i+1,crosspoint(1)+zhi);

pop(i,j)=y;

end

end

end

[pop]=chap10_1dis(pop);

for i=1:s

if pop1(i,t)

pop(i,:)=pop1(i,:);

end

end

变异算子函数：

function [pop]=mutate(pop)

[s,t]=size(pop);

pop1=pop;

for i=1:2:s

m=randperm(t-3)+1; %去除城市队列的头和尾，头尾是一个城市

mutatepoint(1)=min(m(1),m(2));

mutatepoint(2)=max(m(1),m(2));

mutate=round((mutatepoint(2)-mutatepoint(1))/2-0.5);

for j=1:mutate

zhong=pop(i,mutatepoint(1)+j);

pop(i,mutatepoint(1)+j)=pop(i,mutatepoint(2)-j);

pop(i,mutatepoint(2)-j)=zhong;

end

end

[pop]=chap10_1dis(pop);

for i=1:s

if pop1(i,t)

pop(i,:)=pop1(i,:);

end

end