fitness.m

粒子群(PSO)路径规划。这个是一个局部路径规划

%--------适应值函数--------------------------------------------------
function result=fitness(x,m)
%--声明全局------------
global fitw1;
global fitw2;
global segR;
global g;
%--

sum = 0;
for i = 1:m
    if 1 == i
        tempdis = segR;
    else
        tempdis = sqrt( ((i-1)*segR)^2+(i*segR)^2-...
                        2*((i-1)*segR)*(i*segR)*...
                            cos(x(i-1)-x(i)) );
    end
    sum = sum + tempdis;
end
[tempPoint(1),tempPoint(2)]=plorTozhijiao(i*segR,x(i));
dist = distance(tempPoint(1),tempPoint(2),g(1),g(2));
sum = fitw1*sum + fitw2*dist;
result=sum;