clc; clear; close all; delete('Y.txt'); p1 = 45; p2 = 5; rt =0.2; nobs = 10; a = [10, 10, 15, 25, 35, 45, 5, 10]; b = [5, 15, 30, 30, 30, 35, 30, 40]; o1 =[18, 6, 30, 39, 13, 10.5, 34, 4, 13, 22.5]; o2 = [16, 5, 35.5, 32, 31, 6, 5, 11, 25, 1.5]; ro = [0.4, 0.95, 0.9, 0.05, 1.8, 1, 0.8, 1, 2, 1]; x(1) = 5; x(2) = 45; x(3) = 0; x(4) = 1; x(5) = 1; %% Problem Definition CostFunction=@(x,s, o1,o2,nobs,a ,b) Objective(x,s,o1,o2,nobs,a,b); % Cost Function nVar=2; % Number of Decision Variables VarSize=[1 nVar]; % Variables Matrix Size VarMin=0; % Decision Variables Lower Bound VarMax= 50; % Decision Variables Upper Bound %% ACOR Parameters MaxIt=1000; % Maximum Number of Iterations nPop=500; % Population Size (Archive Size) nSample=10000; % Sample Size q=0.5; % Intensification Factor (Selection Pressure) zeta=1; % Deviation-Distance Ratio %% Initialization % Create Empty Individual Structure empty_individual.Position=[]; empty_individual.Cost=[]; source.position(1) = 5; source.position(2) = 45; temp.position(1) = 5; temp.position(2) = 45; distance = 0; % Create Population Matrix pop=repmat(empty_individual,nPop,1); % Initialize Population Members for i=1:nPop % Create Random Solution pop(i).Position=unifrnd(VarMin,VarMax,VarSize); % Evaluation pop(i).Cost=CostFunction(pop(i).Position, source.position, o1,o2,nobs,a,b); end %Draw obstacle t = linspace(0,2*pi,1000); for i = 1:nobs ho(i) = fill(ro(i)*cos(t)+o1(i),ro(i)*sin(t)+o2(i),'.'); set(ho(i),'MarkerSize',1, 'FaceColor','r'); hold on end u = linspace(a(1),a(2),2); y = linspace(b(1),b(2),2); line(u,y); c = linspace(a(3),a(4),2); d = linspace(b(3),b(4),2); line(c,d); e = linspace(a(5),a(6),2); f = linspace(b(5),b(6),2); line(e,f); g = linspace(a(7),a(8),2); h = linspace(b(7),b(8),2); line(g,h); ht = plot(rt*cos(t)+p1,rt*sin(t)+p2,'.');set(ht,'MarkerSize',10); hs = plot(rt*cos(t)+x(1),rt*sin(t)+x(2),'.');set(ht,'MarkerSize',10); hold on h = plot(x(1),x(2),'.','MarkerSize',18); hf = plot(x(1),x(2),'.','MarkerSize',1); % Sort Population [~, SortOrder]=sort([pop.Cost]); pop=pop(SortOrder); % Update Best Solution Ever Found BestSol=pop(1); % Array to Hold Best Cost Values BestCost=zeros(MaxIt,1); % Solution Weights w=1/(sqrt(2*pi)*q*nPop)*exp(-0.5*(((1:nPop)-1)/(q*nPop)).^2); % Selection Probabilities p=w/sum(w); %% ACOR Main Loop tic; for it=1:MaxIt % Means s=zeros(nPop,nVar); for l=1:nPop s(l,:)=pop(l).Position; end % Standard Deviations sigma=zeros(nPop,nVar); for l=1:nPop D=0; for r=1:nPop D=D+abs(s(l,:)-s(r,:)); end sigma(l,:)=zeta*D/(nPop-1); end % Create New Population Array newpop=repmat(empty_individual,nSample,1); for t=1:nSample % Initialize Position Matrix newpop(t).Position=zeros(VarSize); % Solution Construction for i=1:nVar % Select Gaussian Kernel l=RouletteWheelSelection(p); % Generate Gaussian Random Variable newpop(t).Position(i)=s(l,i)+sigma(l,i)*randn; end % Evaluation newpop(t).Cost=CostFunction(newpop(t).Position, source.position,o1,o2, nobs, a, b); end % Merge Main Population (Archive) and New Population (Samples) pop=[pop newpop]; %#ok % Sort Population [~, SortOrder]=sort([pop.Cost]); pop=pop(SortOrder); % Delete Extra Members pop=pop(1:nPop); % Update Best Solution Ever Found BestSol=pop(1); % Store Best Cost BestCost(it)=BestSol.Cost; % Show Iteration Information source.position = BestSol.Position; % Show Iteration Information disp(['Iteration ' num2str(source.position) ': Best Cost = ' num2str(BestCost(it))]); A = [temp.position(1) source.position(1)]; B = [temp.position(2) source.position(2)]; plot(A,B,'*'); drawnow; ttheta = 0; tolerance = 0.1; distTP = sqrt((source.position(1) -temp.position(1))^2 + ((temp.position(2) - source.position(2) )^2)); while distTP > tolerance stepsize1=0.1; stepsize = 0.1; alpha1 = 0.1; alpha2= 0.1; u1=alpha1*(source.position(1)-x(1)); u2=alpha2*(source.position(2)-x(2)); %Kinematic equations Fx(1) = u1; Fx(2) = u2; %RK4 to solve for x(1),x(2): for I = 1:2 kx1(I) = stepsize * Fx(I); x(I) = x(I) + kx1(I) / 2; kx2(I) = stepsize * Fx(I); x(I) = x(I) + kx2(I) / 2; kx3(I) = stepsize * Fx(I); x(I) = x(I) + kx3(I) / 2; x(I) = x(I) + (kx1(I) + 2 * (kx2(I) + kx3(I)) + stepsize * Fx(I)) / 6; end distance = distance + (sqrt(((temp.position(1) - x(1) )^2)+((temp.position(2) - x(2) )^2))); temp.position(1) = x(1); temp.position(2) = x(2); drawnow; set(h,'XData',x(1),'YData',x(2)) %Write the data values to files dlmwrite('Y.txt',[x(1),x(2)], 'newline', 'pc', '-append') % Draw the path X = dlmread('Y.txt'); set(hf,'XData',X(:,1),'YData',X(:,2)) distTP = sqrt((source.position(1) -x(1))^2 + ((x(2) - source.position(2) )^2)); end distSP = sqrt((source.position(1) -p1)^2 + ((p2 - source.position(2) )^2)); if distSP <= 0.001 break; end temp.position = source.position; disp(['Distance ' num2str(distance) ]); end toc; %% Results figure; %plot(BestCost,'LineWidth',2); semilogy(BestCost,'LineWidth',2); xlabel('Iteration'); ylabel('Best Cost'); grid on; function z=Objective(x,s,o1,o2, nobs, a, b) %Target position p1 = 45; p2 = 5; rt =0.2; u = linspace(a(1),a(2),10); y = linspace(b(1),b(2),10); c = linspace(a(3),a(4),10); d = linspace(b(3),b(4),10); e = linspace(a(5),a(6),10); f = linspace(b(5),b(6),10); g = linspace(a(7),a(8),10); h = linspace(b(7),b(8),10); distFR = (sqrt(((s(1) - x(1))^2)+((s(2) - x(2))^2) )) ; if distFR > 3 z = inf; else z = 0; k = 0; m = 0; for i = 1: nobs z = z + (1/sqrt(((o1(i) - x(1))^2)+((o2(i) - x(2))^2) )); end for j = 1:10 m = m + (1/sqrt(((u(j) - x(1))^2)+((y(j) - x(2))^2) ))+ (1/sqrt(((c(j) - x(1))^2)+((d(j) - x(2))^2) ))+(1/sqrt(((e(j) - x(1))^2)+((f(j) - x(2))^2) )) + (1/sqrt(((g(j) - x(1))^2)+((h(j) - x(2))^2) )); %use minimum distance technique end z = 0.01*(z) + 0.01*(m)+ 0.01*(sqrt(((p1 - x(1) )^2)+((p2 - x(2) )^2))); end end