机器人路径规划 转自《机器人控制系统的设计与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<k+1
   [a,mmax(num)]=max(m11);
   m11(mmax(num))=0;
   num=num+1;
end

num=1;
while num<k+1
   [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,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,t)
      pop(i,:)=pop1(i,:);
   end
end