二维_避障.zip_多智能体_多智能体 避障_多避障_智能体_智能体避障

clear clc N = 100;l = 6; d = 7; d_ob = 0.6*d; r = d*1.2;r_ob = 1.2*d_ob; step = 0.02; T = 15000; h = 0.2;h_ob = 0.9; epsilon = 0.1; a = 5;b = 5; c1_alpha = 1; c2_alpha = 2*sqrt(c1_alpha); c1_beta = 1; c2_beta = 2*sqrt(c1_beta); c1_gama = 0.01; c2_gama = 2*sqrt(c1_gama); Q = -20 + 20*randn(N,2);%sqrt(2500)* P = zeros(N,2); M = [100 110 120 130 150 160;20 60 40 -20 40 0;7 4 2 3 5 3]; % M = [10 20 30 40 50 60;20 60 40 -20 40 0;10 4 2 5 5 3]; qr = [100 20]; pr = [1 0.25]; d_alpha = sigma_norm(d,epsilon);d_beta = sigma_norm(d_ob,epsilon); r_alpha = sigma_norm(r,epsilon);r_beta = sigma_norm(r_ob,epsilon); cut_off = bump_function(d_alpha/r_alpha,h);cut_off_ob = bump_function(d_beta/r_beta,h_ob); v =VideoWriter('Fig_10.avi'); open(v); for k = 1:1:T u = zeros(N,2); Adj = adjacency(Q,r_alpha,epsilon,h); [Q_ob,P_ob] = beta_agents(M,Q,P,N,l); Ad_Obst = adj_obst(Q_ob,d_beta,Q,epsilon,h_ob,N,l); for i = 1:1:N %% [m, n] = find(Adj(i,:)>0); for j = 1:1:length(n) delta_qij = sigma_norm(Q(n(j),:)-Q(i,:),epsilon); phyalpha = phy_alpha(delta_qij,r_alpha,d_alpha,h,a,b); nij = (Q(n(j),:)-Q(i,:))/sqrt(1+epsilon*delta_qij^2); nij = (Q(n(j),:)-Q(i,:))/sqrt(1+epsilon*norm(Q(n(j),:)-Q(i,:))^2); u(i,:) = u(i,:) + c1_alpha*phyalpha*nij + c2_alpha*Adj(i,n(j))*(P(n(j),:)-P(i,:)); end %% % [q_ob,p_ob] = beta_agents(M,Q(i,:),P(i,:)); % Ad_Obst = adj_obst(q_ob,r_beta,Q(i,:),epsilon,h_ob); [m_ob, n_ob] = find(Ad_Obst(i,:)>0); for j_ob = 1:1:length(n_ob) delta_qij_ob = sigma_norm([Q_ob(i,n_ob(j_ob),1),Q_ob(i,n_ob(j_ob),2)]-Q(i,:),epsilon); sigma_1 = (delta_qij_ob-d_beta)/sqrt(1+(delta_qij_ob-d_beta)^2); phybeta = bump_function(delta_qij_ob/d_beta,h_ob)*(sigma_1 - 1); nij_ob = ([Q_ob(i,n_ob(j_ob),1),Q_ob(i,n_ob(j_ob),2)]-Q(i,:))/sqrt(1+epsilon*norm([Q_ob(i,n_ob(j_ob),1),Q_ob(i,n_ob(j_ob),2)]-Q(i,:))^2); % nij_ob = ([Q_ob(i,n_ob(j_ob),1),Q_ob(i,n_ob(j_ob),2)]-Q(i,:))/sqrt(1+epsilon*delta_qij_ob^2); u(i,:) = u(i,:) + c1_beta*phybeta*nij_ob + c2_beta*Ad_Obst(i,n_ob(j_ob))*([P_ob(i,n_ob(j_ob),1),P_ob(i,n_ob(j_ob),2)] - P(i,:)); end % %% u(i,:) = u(i,:) -c1_gama*((Q(i,:)-qr)/sqrt(1+norm(Q(i,:)-qr))^2)-c2_gama*(P(i,:)-pr);% end Q = Q + P*step; P = P + u*step; qr = qr + pr*step; % plot_flocking(Adj,Obst,P,Q,M,cut_off); plot_flocking(Adj,P,Q,cut_off,M,Ad_Obst); frame = getframe; pause(0.0015); writeVideo(v,frame); end