人工势场法的MATLAB的仿真程序.rar

function [route] = APF(InputMap,StartNode,GoalNode) [X,Y] = meshgrid(1:100,1:100); map = InputMap; Fatt = zeros(100); Freq = zeros(100); Katt = 0.3; Kreq = 30; DoRange = 10; RANGE = []; for x=1:100 for y=1:100 Fatt(x,y) = Katt*sqrt((abs(x-GoalNode(1)))^2+(abs(y-GoalNode(2))^2)); end end for x=1:100 for y=1:100 RANGE = []; MinDist = Inf; for i=0:DoRange for j=0:DoRange if (i^2+j^2)<=DoRange^2 && x-i>0 && y-j>0 && x+i<=100 && y+j<=100 if map(x+i,y+j)==1 RANGE = [RANGE;[x+i,y+j]]; end if map(x+i,y-j)==1 RANGE = [RANGE;[x+i,y-j]]; end if map(x-i,y+j)==1 RANGE = [RANGE;[x-i,y+j]]; end if map(x-i,y-j)==1 RANGE = [RANGE;[x-i,y-j]]; end end end end if (RANGE) MinDist = min(sqrt(abs(RANGE(:,1)-x).^2 + abs(RANGE(:,2)-y).^2)); Freq(x,y) = Kreq*(1/MinDist - 1/DoRange); % Freq(x,y) = Kreq*(1/MinDist - 1/DoRange)*(1/MinDist^2); continue; else Freq(x,y) = 0; end end end Freq(Freq==Inf)=30; F = Fatt+Freq; route = [StartNode]; power = F(StartNode(1),StartNode(2)); direct = [1,0; 1,-1; 0,-1; -1,-1; -1,0; -1,1; 0,1; 1,1]; n=0; NUM = 0; flag = false; while (route(end,1)~=GoalNode(1) || route(end,2)~=GoalNode(2)) && NUM<200 NUM = NUM+1; for i=1:length(direct(:,1)) CurrentLocation = route(end,:)+direct(i,:); if power > min(power,F(CurrentLocation(1),CurrentLocation(2))) flag = true; power = min(power,F(CurrentLocation(1),CurrentLocation(2))); n = i; end end if (flag) route = [route;[route(end,:)+direct(n,:)]]; flag = false; end end for i=1:length(route(:,1)) F(route(i,1),route(i,2)) = F(route(i,1),route(i,2))+30; end surf(X,Y,F); figure contour(F,100);