ymfitness1.m

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

function P=ymfitness1(P,d)
[n1,n2]=size(P);
A=P;
a=zeros(n1,1);
b=zeros(n1,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);
    I=orange(A(i,j),A(i,j+1),d);
    b(i)=b(i)+I;
    A(i,2)=b(i);
    
    end
end
P(:,1)=A(:,1);
P(:,2)=A(:,2);