Flocking for Multi-Agent Dynamic Systems

% 本文参照文献：Flocking for Multi-Agent Dynamic Systems:Algorithms and Theory clear; close all; clc; %% Parameters 初始化参数 num_agents = 100; t_gap=1; % 迭代间隔 queue_gap=15; % 队形间隔 queue_vy=12; queue_vx=13; queue_r=40; r_c=20; % 交互范围（半径） k=1.2; % 晶格的ratio d=r_c/k; % 晶格的scale(表示两两智能体之间的距离（论文中公式5）) v_0=2; % 初始速度 v_limit=0; % 最大速度 efs = 1; % sigma-norms parameter h=0.4; % 设置bump function的分割点（公式10） d_o = r_c; r_c_sigma = sigma_norm(r_c,efs); % r_c的σ范数 d_sigma = sigma_norm(d,efs); % d的σ范数 map_width = 400; % width of a squre map map_res = 0.5; % width of a grid to play obstacles pixel c1=0.2;c2=0.5;c3=0.2;c4=0.1;c5=10;c6=0.01; x = zeros(num_agents,2); % current position x_1 = zeros(num_agents,2); % previous position v_1 = zeros(num_agents,2); % previous velocity x_r0= zeros(num_agents,2); % x_r0：用来存储指定的队形信息 v_r_1st_point=300; path_num = zeros(1000,2,num_agents); v_r=[1,0];