ymfitness1.asv

规则障碍物环境下的机器人路径规划matlab源码！

function P=ymfitness1(P,d)
[n1,n2]=size(P);
A=P;
a=zeros(m1,1);
for i=1:n1
    for j=3:2:n2-2
    a(i)=a(i)+sqrt((A(i,j+3)-A(i,j+1)).^2+(A(i,j+2)-A(i,j)).^2) ;  
    A(i,1)=a(i);
    b()=orange(A(i,j),A(i,j+1),d)
    end
end
P(:,1)=A(:,1);