├── ConfigFile.m ├── Control.m ├── DistPt2Ln_thetaP.m ├── DrawBearing.m ├── DrawCircle.m ├── DrawWorld.m ├── Initialize.m ├── MakeColorMap.m ├── Move.m ├── README.md ├── Sense.m ├── Simulator.m ├── StandardizeAngle.m ├── log.mat ├── obs.m └── run.m /ConfigFile.m: -------------------------------------------------------------------------------- 1 | function [config, sensor, form, obs] = ConfigFile() 2 | %% --------------------------------------------------% 3 | % --------------------------------------------------- input 4 | % --------------------------------------------------- output 5 | % config - parameters for robots 6 | % sensor - parameters for sensors 7 | % form - parameters for formation control 8 | % obs - parameters for obs 9 | % --------------------------------------------------- 10 | 11 | %% -------------------------------------------------- 12 | % General Parameters 13 | % --------------------------------------------------- 14 | scale = 1; 15 | 16 | config.movie = 0; % movie - 1 then make movie, 0 not 17 | config.pause = 0; 18 | 19 | %% -------------------------------------------------- 20 | % config - robots 21 | % --------------------------------------------------- 22 | config.T = 10^3; % total time steps 23 | config.num_Robots = 50; % total number of robots 24 | 25 | config.dim_X = 2; % [x, y, theta] 26 | config.dim_L = 2; % [range, bearing] 27 | 28 | % draw 29 | config.siz_Fig = 2000 * scale; % size of the figure to draw 30 | config.color_Map = MakeColorMap([1 0 0], ... % red [1 0 0] 31 | [0 1 0], ... % green [0 1 0] 32 | [0 0 1], ... % blue [0 0 1] 33 | config.num_Robots); 34 | config.style_Rob = 'o'; 35 | config.color_Bearing = 'r'; 36 | config.style_Bearing = '-'; 37 | config.len_Bearing = 20 * scale; 38 | 39 | % region for initialization 40 | config.siz_Ini = [0, config.siz_Fig;%/2; % initialize robots to be 41 | 0, config.siz_Fig];%/2]; % within this region 42 | % size 43 | config.rad_Rob = 10 * scale; % 44 | % motion 45 | config.u_Ini = 12; 46 | config.u_Min = 10; 47 | config.u_Max = 15; 48 | config.dt = 1; 49 | 50 | config.straight_Thres = deg2rad(1); % w - straight motion or not 51 | 52 | % for obstacle 53 | config.num_Obs = 0; 54 | config.siz_Obs = 200*ones(config.num_Obs,1);%[20, 50, 80]'; % range of size of obs 55 | config.color_Obs = 'k'; 56 | config.style_Obs = '--'; 57 | %% -------------------------------------------------- 58 | % formation 59 | % --------------------------------------------------- 60 | form.id = 2; % what formation to use for the fleet: formation(formationID, :) 61 | 62 | %% -------------------------------------------------- 63 | % sensor 64 | % --------------------------------------------------- 65 | sensor.n = 3; % 1 - laser; 2 - radar; 3 - vision 66 | sensor.range = 200 * scale; 67 | sensor.bearing = deg2rad(180); 68 | sensor.color = 'r'; % now useing robot-specific color/draw the sensing area 69 | sensor.style = '--'; 70 | 71 | end 72 | 73 | -------------------------------------------------------------------------------- /Control.m: -------------------------------------------------------------------------------- 1 | function [data, form] = Control(t, data, config, sensor, form) 2 | %% --------------------------------------------------% 3 | % --------------------------------------------------- input 4 | % t - current time step 5 | % data - data.p - T x (nR x 3) mat - [x, y] - up to (t-1) 6 | % - data.u - T x (nR x 2) mat - [vx, vy] - up to (t-1) 7 | % - data.z - T x (nR) cell - n x [id] - up to (t-1) 8 | % - data.zO- T x (nR) cell - n x [id] - up to (t-1) 9 | % config - num_Robots/dim_X 10 | % sensor - range 11 | % form - id 12 | % --------------------------------------------------- output 13 | % data - data.p - T x (nR x 3) mat - [x, y] - up to (t-1) 14 | % - data.u - T x (nR x 2) mat - [vx, vy] - up to (t) 15 | % - data.z - T x (nR) cell - n x [id] - up to (t-1) 16 | % - data.zO- T x (nR) cell - n x [id] - up tp (t-1) 17 | % --------------------------------------------------- 18 | 19 | if t == 2 20 | data.u(t, :) = data.u(t-1, :); % no move, haven't sensed 21 | return; 22 | end 23 | 24 | for i = 1 : config.num_Robots 25 | % --------------------------------------------- % at t, use z(t-1) to get u(t) then p(t) 26 | % --------------------------------------------- % obs detected 27 | o = data.zO{t-1,i}; % n x [x, y, siz] 28 | % --------------------------------------------- % of itself 29 | p = data.p(t-1, (config.dim_X * (i-1) + 1) : ... 30 | (config.dim_X * (i-1) + config.dim_X)); 31 | u = data.u(t-1, (2 * (i-1) + 1) : ... 32 | (2 * (i-1) + 2)); 33 | % --------------------------------------------- % of its neighbour 34 | z = data.z{t-1, i}; 35 | pN = [data.p(t-1, config.dim_X * z - 1)' ... 36 | data.p(t-1, config.dim_X * z )']; 37 | uN = [data.u(t-1, 2 * z - 1)' ... 38 | data.u(t-1, 2 * z )']; 39 | % --------------------------------------------- % of all except itself 40 | pA = [data.p(t-1, 1 : config.dim_X : (end-1))' ... 41 | data.p(t-1, 2 : config.dim_X : (end ))']; 42 | uA = [data.u(t-1, 1 : 2 : (end-1))' ... 43 | data.u(t-1, 2 : 2 : (end ))']; 44 | % --------------------------------------------- % for formation 45 | % --------------------------------------------- % rules 46 | numRules = 5; 47 | du = zeros(numRules, 2); 48 | 49 | % Dynamic factors that determined by specific distribution in the 50 | % sensor range 51 | w = zeros(numRules, 1); 52 | 53 | % Manually set scale factors. 54 | % It's a constant that makes du to a resonable scale. 55 | scale = ones(numRules, 1); 56 | scale(1) = 5; 57 | scale(2) = 0; 58 | scale(3) = 1; 59 | scale(4) = 0.1; 60 | scale(5) = 5; 61 | 62 | [du(1, :), w(1)] = Obstacle(p, u, o, config.rad_Rob, sensor.range); 63 | [du(2, :), w(2)] = Separation(p, pN, u, uN, config.rad_Rob); 64 | [du(3, :), w(3)] = Alignment(u, uN); 65 | [du(4, :), w(4)] = Cohesion(p, pN); 66 | if form.id == 1 67 | [du(5, :)] = I_Form(200, p, u, pN, uN); 68 | w(5) = 1; 69 | end 70 | if form.id == 2 && t == 3 % leader for V formation 71 | pM = [sum(pA(:, 1)), sum(pA(:, 2))] / size(pA, 1); 72 | dist = pdist2(pM, pA); 73 | [~, form.leader.id] = min(dist); 74 | end 75 | if form.id == 2 76 | form.leader.p = pA(form.leader.id, :); 77 | form.leader.theta = atan2(uA(form.leader.id, 2), uA(form.leader.id, 1)); 78 | end 79 | if form.id == 2 80 | du(5, :) = V_Form(20, p, uN, deg2rad(45), form.leader); 81 | w(5) = 1; 82 | end 83 | % --------------------------------------------- % update 84 | 85 | % Obstacle avoidance has the highest priority. 86 | % If the robot is too close to the obstacle, it won't care about 87 | % other rules and will only avoides the obstacle, 88 | % which could 'push' others away due to the seperation rule. 89 | if w(1) == 1 90 | u = du(1, :) * norm(u); 91 | else 92 | % Seperation rule has the second highest priority. If the robot 93 | % is not too close to an obstacle but too close to 94 | % another robot, it won't care about the rest rules and will only 95 | % keep away from that robot before it can continue aligning and 96 | % cohering with others. 97 | if w(2) == 1 98 | u = du(2, :) * norm(u); 99 | else 100 | % 101 | if w(2) * scale(2) < (1-0.618) 102 | w(2) = 1 - 0.618; 103 | scale(2) = 1; 104 | else 105 | % 106 | if w(2) * scale(2) > 1 107 | w(2) = 1; 108 | scale(2) = 1; 109 | end 110 | end 111 | % univector of dynamically and constantly weighed average du after all rules 112 | du = mean(du .* repmat(w, 1, 2) .* repmat(scale, 1, 2), 1); 113 | 114 | % Only provide a small portion of current speed to change. 115 | % let new speed always has the same value as the old, only 116 | % change the direction 117 | norm_u = norm(u); 118 | u = 0.618* u + (1) * norm(u) * du; 119 | u = u / norm(u) * norm_u; 120 | end 121 | end 122 | 123 | % limit the speed within [u_Min u_Max] 124 | normU = norm(u); 125 | if normU > config.u_Max 126 | u = config.u_Max * u ./ normU; 127 | end 128 | if normU < config.u_Min && normU >0; 129 | u = config.u_Min * u ./ normU; 130 | end 131 | 132 | % update the speed 133 | data.u(t, (2 * (i-1) + 1) : (2 * (i))) = u; 134 | end 135 | 136 | end 137 | 138 | function [du, w] = Obstacle(p, u, o, rad_Rob, sensor_Range) 139 | %% --------------------------------------------------% 140 | % Obstacle - obstacle avoidance 141 | % --------------------------------------------------- input 142 | % p - 1 x [x, y] - position of itself 143 | % u - 1 x [vx, vy] - velocity of itself 144 | % o - n x [x, y, siz] - obs detected 145 | % - within sensing range & bearing(only those in front) 146 | % rad_Rob 147 | % sensor_Range 148 | % --------------------------------------------------- output 149 | % du - [dvx, dvy] 150 | % - velo turbulence under this rule 151 | % w - between [0, 1] 152 | % - 1 if < thres 153 | % --------------------------------------------------- 154 | 155 | if isempty(o) 156 | du = [0, 0]; 157 | w = 0; 158 | return; 159 | else 160 | numO = size(o, 1); 161 | du = zeros(numO, 2); 162 | w = zeros(numO, 1); 163 | 164 | for i = 1 : size(o, 1) 165 | theta = atan2(u(2), u(1)); % bearing of the robot 166 | distRO = pdist2(p, o(i, 1 : 2)); % dist between center of r & o 167 | dist = DistPt2Ln_thetaP(o(i, 1 : 2), theta, p); % dist from o center to proceeding line of r 168 | r = o(i, 3) + rad_Rob + 10; % expand o with rad of r, and a redun 10 169 | if abs(dist) < r % possible to collide 170 | % ----------------------------------------- % get len 171 | len = sqrt(distRO^2 - dist^2) - sqrt(r^2 - dist^2); 172 | % ----------------------------------------- % get beta 173 | beta = asin(abs(dist)/r); 174 | if dist > 0 % obs is on the left of the robot 175 | beta = theta - (pi/2 - beta); 176 | else % obs is on the right of the robot 177 | beta = theta + (pi/2 - beta); 178 | end 179 | % ----------------------------------------- % get du(i, :) & w(i) 180 | lenSafe = norm(u); 181 | du(i, :) = [cos(beta), sin(beta)]; 182 | if len < lenSafe 183 | % too close to the ith obstacle, weight to avoid this obstacle = 1 184 | du(i, :) = [cos(beta), sin(beta)]; 185 | w(i) = 1; 186 | else 187 | % Not too close to the ith obstacle. So give a du to push 188 | % current speed to the tagent speed which can lead the robot 189 | % avoid the obstacle. 190 | du(i, :) = [cos(beta), sin(beta)] - u / norm(u); 191 | du(i, :) = du(i, :) / norm(du(i, :)); 192 | 193 | % distRO will not exceed obstacle radius plus sensor range. 194 | % closer to the obstacle has higher weight (0~1). 195 | w(i) = (o(i,3)+sensor_Range-distRO) / (o(i,3)+sensor_Range); 196 | end 197 | else 198 | % will not collide, set the weight to -1 temporarily 199 | w(i) = -1; 200 | end 201 | end 202 | % filter out the conditions that will not collide 203 | du = du(w~=-1, :); 204 | w = w(w~=-1); 205 | 206 | % there is no obstacle that will collide in range. 207 | if isempty(w) 208 | du = [0, 0]; 209 | w = 0; 210 | else 211 | % if there exists at least one obstacle have to avoid urgently 212 | if sum(w==1) 213 | du = mean(du(w==1, :), 1); 214 | w = 1; 215 | % there is no obstacle that has to avoid urgently 216 | else 217 | % weighted average of all du to avoid obstacles 218 | du = mean(du .* repmat(w ./ sum(w), 1, 2), 1); 219 | w = mean(w); 220 | end 221 | % get the univector of du. 222 | du = du / norm(du); 223 | end 224 | end 225 | 226 | end 227 | 228 | function [du, w] = Separation(p, pN, u, uN, rad_Rob) 229 | %% --------------------------------------------------% 230 | % Separation - avoid crowding local flockmates 231 | % --------------------------------------------------- input 232 | % p - [x, y] 233 | % pN - n x [x, y] 234 | % u - [vx, vy] 235 | % uN - n x [vx, vy] 236 | % rad_Rob 237 | % --------------------------------------------------- output 238 | % du - [dvx, dvy] 239 | % - velo turbulence under this rule 240 | % w 241 | % --------------------------------------------------- 242 | 243 | if isempty(pN) 244 | du = [0, 0]; 245 | w = 0; 246 | else 247 | numN = size(pN, 1); 248 | du = zeros(numN, 2); 249 | w = zeros(numN, 1); 250 | 251 | tmpP = repmat(p, numN, 1); 252 | tmp = tmpP - pN; 253 | 254 | normT = zeros(numN, 1); 255 | for i = 1 : numN 256 | normT(i) = norm(tmp(i, :)); 257 | du(i, :) = tmp(i, :) ./ (normT(i)^3); 258 | w(i) = 1 / (normT(i)^2); 259 | end 260 | du = sum(du) / norm(sum(du)); 261 | if min(normT) < 2 * rad_Rob 262 | w = sum(w); %1; 263 | else 264 | w = sum(w); 265 | end 266 | end 267 | 268 | end 269 | 270 | function [du, w] = Alignment(u, uN) 271 | %% --------------------------------------------------% 272 | % Alignment - steer towards the average heading of local flockmates 273 | % --------------------------------------------------- input 274 | % u - [vx, vy] 275 | % uN - n x [vx, vy] 276 | % --------------------------------------------------- output 277 | % du - [dvx, dvy] 278 | % - velo turbulence under this rule 279 | % w 280 | % --------------------------------------------------- 281 | 282 | du = [0, 0]; 283 | w = 0; 284 | if ~isempty(uN) 285 | du = ([sum(uN(:, 1)) sum(uN(:, 2))] / size(uN, 1) - u); 286 | w = (1-0.618);%norm(du); 287 | du = du ./ norm(du); 288 | end 289 | end 290 | 291 | function [du, w] = Cohesion(p, pN) 292 | %% --------------------------------------------------% 293 | % Cohesion - steer to move toward the average position of local flockmates 294 | % --------------------------------------------------- input 295 | % p - [x, y] 296 | % pN - n x [x, y] 297 | % --------------------------------------------------- output 298 | % du - [dvx, dvy] 299 | % - velo turbulence under this rule 300 | % w 301 | % --------------------------------------------------- 302 | 303 | du = [0, 0]; 304 | w = 0; 305 | if ~isempty(pN) 306 | du = ([sum(pN(:, 1)) sum(pN(:, 2))] / size(pN, 1) - p); 307 | w = 1;%0.618; 308 | du = du ./ norm(du); 309 | end 310 | end 311 | 312 | function du = I_Form(scale, p, u, pA, uA) 313 | %% --------------------------------------------------% 314 | % I_Form - proceed in line formation 315 | % --------------------------------------------------- input 316 | % scale - control the convergence speed 317 | % p - [x, y] 318 | % u - [vx, vy] 319 | % pA - n x [x, y] 320 | % uA - n x [vx, vy] 321 | % --------------------------------------------------- output 322 | % du - [dvx, dvy] 323 | % - velo turbulence under this rule 324 | % --------------------------------------------------- 325 | 326 | du = [0, 0]; 327 | if ~isempty(uA) 328 | uC = ([sum(uA(:, 1)) sum(uA(:, 2))] + u) / (size(uA, 1) + 1); % motion of center 329 | pC = ([sum(pA(:, 1)) sum(pA(:, 2))] + p) / (size(pA, 1) + 1); % position of center 330 | thetaC = atan2(uC(2), uC(1)); 331 | hold on; 332 | % DrawBearing(pC(1), pC(2), thetaC, 30, 'r', '-'); 333 | dist = DistPt2Ln_thetaP(p, thetaC, pC); % >0 p on the left side of the line 334 | if dist < 0 % on the right 335 | % DrawBearing(p(1), p(2), thetaC+pi/2, -dist, 'b', '--'); 336 | du = abs(dist) * [-sin(thetaC), cos(thetaC)]; 337 | else % on the left 338 | % DrawBearing(p(1), p(2), thetaC-pi/2, dist, 'b', '--'); 339 | du = abs(dist) * [ sin(thetaC), -cos(thetaC)]; 340 | end 341 | du = du / scale; 342 | end 343 | 344 | end 345 | 346 | function du = V_Form(scale, p, uA, theta, leader) 347 | %% --------------------------------------------------% 348 | % V_Form - proceed in inverse V formation 349 | % --------------------------------------------------- input 350 | % scale - control the convergence speed 351 | % p - [x, y] 352 | % uA - n x [vx, vy] 353 | % leader- leader.id 354 | % - leader.p 355 | % - leader.theta 356 | % --------------------------------------------------- output 357 | % du - [dvx, dvy] 358 | % - velo turbulence under this rule 359 | % --------------------------------------------------- 360 | 361 | du = [0, 0]; 362 | if ~isempty(uA) 363 | thetaC = leader.theta; % motion of center 364 | pC = leader.p; % position of center 365 | % hold on; 366 | % DrawBearing(pC(1), pC(2), thetaC+pi, 30, 'r', '-'); 367 | % DrawBearing(pC(1), pC(2), thetaC-theta+pi, 200, 'r', '-'); 368 | % DrawBearing(pC(1), pC(2), thetaC+theta+pi, 200, 'r', '-'); 369 | 370 | thetaL = thetaC - theta; 371 | thetaR = thetaC + theta; 372 | distL = DistPt2Ln_thetaP(p, thetaL, pC); % >0 p on the left side of the line 373 | distR = DistPt2Ln_thetaP(p, thetaR, pC); % >0 p on the left side of the line 374 | 375 | dist = DistPt2Ln_thetaP(p, thetaC, pC); 376 | if abs(dist) >= 0.01 % not the leader 377 | if dist < 0 % should converge to the right line 378 | if distR < 0 379 | % DrawBearing(p(1), p(2), thetaR+pi/2, -distR, 'b', '--'); 380 | du = abs(distR) * [-sin(thetaR), cos(thetaR)]; 381 | else 382 | % DrawBearing(p(1), p(2), thetaR-pi/2, distR, 'b', '--'); 383 | du = abs(distR) * [ sin(thetaR), -cos(thetaR)]; 384 | end 385 | else 386 | if distL < 0 387 | % DrawBearing(p(1), p(2), thetaL+pi/2, -distL, 'b', '--'); 388 | du = abs(distL) * [-sin(thetaL), cos(thetaL)]; 389 | else 390 | % DrawBearing(p(1), p(2), thetaL-pi/2, distL, 'b', '--'); 391 | du = abs(distL) * [ sin(thetaL), - cos(thetaL)]; 392 | end 393 | end 394 | end 395 | du = du / scale; 396 | end 397 | 398 | end -------------------------------------------------------------------------------- /DistPt2Ln_thetaP.m: -------------------------------------------------------------------------------- 1 | function dist = DistPt2Ln_thetaP(Pt, theta, Pt1) 2 | %% -------------------------------------------------- 3 | % --------------------------------------------------- input 4 | % Pt - [x, y] 5 | % theta - 6 | % Pt1 - [x, y] 7 | % --------------------------------------------------- output 8 | % dist - distance between Pt to Line defined by 9 | % slope k=tan(theta) and Pt1 10 | % - >0 if Pt is on the left side of the line 11 | % - <0 if on the right 12 | % --------------------------------------------------- 13 | 14 | theta1 = atan2(Pt(2) - Pt1(2), Pt(1) - Pt1(1)); 15 | relativeTheta = theta1 - theta; 16 | dist = pdist2([Pt(1), Pt(2)], [Pt1(1), Pt1(2)]); 17 | dist = dist * sin(relativeTheta); 18 | 19 | end -------------------------------------------------------------------------------- /DrawBearing.m: -------------------------------------------------------------------------------- 1 | function DrawBearing(x, y, theta, len_Bearing, colorMap, style_Bearing) 2 | %% --------------------------------------------------% 3 | % --------------------------------------------------- input 4 | % [x, y] - bearing starting from this pt 5 | % theta - orientation of bearing 6 | % len_Bearing - length of bearing to draw 7 | % colorMap - color to draw with 8 | % styleBearing - line style of bearing 9 | % --------------------------------------------------- output 10 | % --------------------------------------------------- 11 | 12 | plot([x + len_Bearing * cos(theta), x], ... 13 | [y + len_Bearing * sin(theta), y], ... 14 | 'Color', colorMap, ... 15 | 'LineStyle', style_Bearing); 16 | 17 | end -------------------------------------------------------------------------------- /DrawCircle.m: -------------------------------------------------------------------------------- 1 | function DrawCircle(cx, cy, r, color_Circle, style_Circle) 2 | %% -------------------------------------------------- 3 | % --------------------------------------------------- input 4 | % cx - n x 1 - center of circle 5 | % cy - n x 1 - center of circle 6 | % r - n x 1 - radius 7 | % color_Circle 8 | % style_Circle 9 | % --------------------------------------------------- output 10 | % --------------------------------------------------- 11 | 12 | for i = 1 : size(cx, 1) 13 | th = linspace(0, 2 * pi, r(i) * 200); 14 | x = r(i) * cos(th) + cx(i); 15 | y = r(i) * sin(th) + cy(i); 16 | plot(x, y, ... 17 | 'Color', color_Circle, ... 18 | 'LineStyle', style_Circle); 19 | hold on; 20 | end 21 | 22 | end -------------------------------------------------------------------------------- /DrawWorld.m: -------------------------------------------------------------------------------- 1 | function DrawWorld(p, u, o, config, sensor) 2 | %% --------------------------------------------------% 3 | % --------------------------------------------------- input 4 | % p - 1 x (config.num_Robots x config.dim_X) vec 5 | % u - 1 x (config.num_Robots x [vx, vy]) vec 6 | % 7 | % config - num_Robots/rad_Rob/ 8 | % color_Map/style_Rob/ 9 | % color_Bearing/style_Bearing 10 | % sensor - 11 | % --------------------------------------------------- output 12 | % --------------------------------------------------- 13 | 14 | hold on; 15 | DrawObs(o, config); 16 | DrawRobots(p, u, config, sensor); 17 | 18 | end 19 | 20 | function DrawObs(o, config) 21 | 22 | hold on; 23 | if ~isempty(o) 24 | DrawCircle(o(:, 1), o(:, 2), o(:, 3), config.color_Obs, config.style_Obs); 25 | end 26 | 27 | end 28 | 29 | function DrawRobots(p, u, config, sensor) 30 | %% --------------------------------------------------% 31 | % --------------------------------------------------- input 32 | % p - 1 x (config.num_Robots x config.dim_X) vec 33 | % u - 1 x (config.num_Robots x [vx, vy]) vec 34 | % config - num_Robots/rad_Rob/ 35 | % color_Map/style_Rob/ 36 | % color_Bearing/style_Bearing 37 | % sensor - 38 | % --------------------------------------------------- output 39 | % --------------------------------------------------- 40 | 41 | %% [LCrevise][Nov.26th][Use matrix calculation instead.] 42 | P = reshape(p, config.dim_X, config.num_Robots)'; 43 | U = reshape(u, config.dim_X, config.num_Robots)'; 44 | X = P(:, 1); 45 | Y = P(:, 2); 46 | UX = U(:, 1); 47 | UY = U(:, 2); 48 | 49 | % This is much faster than n-times calculation. 50 | Theta = atan2(UY, UX); 51 | 52 | for i = 1 : config.num_Robots 53 | % x = p(config.dim_X * (i-1) + 1); 54 | % y = p(config.dim_X * (i-1) + 2); 55 | % theta = atan2(u(2 * (i-1) + 2), u(2 * (i-1) + 1)); 56 | x = X(i); 57 | y = Y(i); 58 | theta = Theta(i); 59 | hold on; 60 | % [plot in a loop is too slow..] 61 | % [Can this be done by matrix operation?] 62 | plot(x, y, ... 63 | 'Color', config.color_Map(i, :), ... 64 | 'LineStyle', config.style_Rob); 65 | % DrawCircle(x, y, ... 66 | % config.rad_Rob, ... 67 | % config.color_Map(i, :), ... 68 | % config.style_Rob); 69 | DrawBearing(x, y, theta, ... 70 | config.len_Bearing, ...%sensor.range, ...%config.len_Bearing, ... 71 | config.color_Map(i, :), ...%config.color_Bearing, ... 72 | config.style_Bearing); 73 | % % if i==1% 74 | % if abs(sensor.bearing - pi) < 0.01 75 | % DrawCircle(x, y, ... 76 | % sensor.range, ... 77 | % config.color_Map(i, :), ... 78 | % sensor.style); 79 | % else 80 | % DrawHalfCircle(x, y, theta, ... 81 | % sensor.range, ... 82 | % sensor.bearing, ... 83 | % config.color_Map(i, :), ... %sensor.color, 84 | % sensor.style); 85 | % end 86 | % % end% 87 | end 88 | % axis([0, config.siz_Fig, 0, config.siz_Fig]); 89 | % %% 90 | % x = p(1 : config.dim_X : end-1); 91 | % y = p(2 : config.dim_X : end); 92 | % theta = atan2(x, y); 93 | % plot(x, y, ... 94 | % 'Color', config.color_Map(randi(config.num_Robots), :), ... 95 | % 'LineStyle', config.style_Rob); 96 | % % DrawBearing(x, y, theta, ... 97 | % % config.len_Bearing, ... 98 | % % config.color_Map(randi(config.num_Robots), :), ... 99 | % % config.style_Bearing); 100 | % 101 | 102 | %% 103 | % --------------------------------------------------- range of fig 104 | % xMin = min(p(1 : config.dim_X : end)); 105 | % xMax = max(p(1 : config.dim_X : end)); 106 | % yMin = min(p(2 : config.dim_X : end)); 107 | % yMax = max(p(2 : config.dim_X : end)); 108 | % if xMax-xMin < config.siz_Fig - 2 * config.rad_Rob 109 | % x1 = (xMin+xMax)/2 - config.siz_Fig/2; 110 | % x2 = (xMin+xMax)/2 + config.siz_Fig/2; 111 | % else 112 | % x1 = xMin - config.rad_Rob; 113 | % x2 = xMax + config.rad_Rob; 114 | % end 115 | % if yMax-yMin < config.siz_Fig - 2 * config.rad_Rob 116 | % y1 = (yMin+yMax)/2 - config.siz_Fig/2; 117 | % y2 = (yMin+yMax)/2 + config.siz_Fig/2; 118 | % else 119 | % y1 = yMin - config.rad_Rob; 120 | % y2 = yMax + config.rad_Rob; 121 | % end 122 | axis([0, config.siz_Fig, 0, config.siz_Fig]); 123 | 124 | end 125 | 126 | function DrawHalfCircle(cx, cy, theta, r, angle, color_Sensor, style_Sensor) 127 | %% --------------------------------------------------% 128 | % --------------------------------------------------- input 129 | % [cx, cy] - center of the circle 130 | % theta - circle pointing at theta 131 | % r - radius 132 | % angle - half of the opening angle of the circle 133 | % color_Sensor - color to draw 134 | % style_Sensor - style to draw 135 | % --------------------------------------------------- output 136 | % --------------------------------------------------- 137 | 138 | th = linspace(theta + angle, theta - angle, 100); 139 | x = r * cos(th) + cx; 140 | y = r * sin(th) + cy; 141 | plot(x, y, 'Color', color_Sensor, 'LineStyle', style_Sensor); 142 | plot([x(1), cx], [y(1), cy], 'Color', color_Sensor, 'LineStyle', style_Sensor); 143 | plot([x(end), cx], [y(end), cy], 'Color', color_Sensor, 'LineStyle', style_Sensor); 144 | 145 | end -------------------------------------------------------------------------------- /Initialize.m: -------------------------------------------------------------------------------- 1 | function [data, obs] = Initialize(t, config) 2 | %% --------------------------------------------------% 3 | % initialize robots - no overlap 4 | % --------------------------------------------------- input 5 | % t - t=1: initial time step 6 | % config - num_Robots/siz_Ini/rad_Rob 7 | % --------------------------------------------------- output 8 | % data - data.p - T x (nR x 3) mat - [x, y] - up to (t) 9 | % - data.u - T x (nR x 2) mat - [vx, vy] - 10 | % - data.z - T x (nR) cell - n x [id] - 11 | % --------------------------------------------------- 12 | 13 | obs = zeros(config.num_Obs, 3); % [x, y, siz] 14 | obs(:, 3) = config.siz_Obs; 15 | 16 | xMin = 0; 17 | xMax = config.siz_Fig; 18 | yMin = 0; 19 | yMax = config.siz_Fig; 20 | 21 | for i = 1 : config.num_Obs 22 | overlap = 1; 23 | while overlap 24 | x = rand() * (xMax - xMin - 2 * obs(i, 3)) + xMin + obs(i, 3); % x 25 | y = rand() * (yMax - yMin - 2 * obs(i, 3)) + yMin + obs(i, 3); % y 26 | if i == 1 27 | overlap = 0; 28 | else 29 | dist = pdist2([x, y], ... 30 | obs(1 : (i-1), 1 : 2)); 31 | if sum(dist' < obs(i, 3) + obs(1 : (i-1), 3) + 100) % at least overlap with one 32 | overlap = 1; 33 | else 34 | overlap = 0; 35 | end 36 | end 37 | end 38 | obs(i, 1 : 2) = [x, y]; 39 | end 40 | 41 | 42 | data.p = zeros(config.T, config.num_Robots * config.dim_X);% T x (nR x [x, y]) 43 | data.u = zeros(config.T, config.num_Robots * 2); % T x (nR x [vx, vy]) 44 | data.z = cell(config.T, config.num_Robots * 1); % T x (nR), entry - n x [id] 45 | data.zO = cell(config.T, config.num_Robots * 1); % T x (nR), entry - n x [x, y, siz] 46 | 47 | % [LC_Revise][Nov.26th][Use rand matrix instead] 48 | % for i = 1 : config.num_Robots 49 | % data.u(t, (2 * (i-1) + 1) : (2 * (i))) = [rand() * 10, rand() * 10]; 50 | % end 51 | 52 | rng('shuffle'); 53 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 54 | % Let the initial velocity in [-10 10] 55 | % PROBLEM: The whole swarm will not move. The swarm velocity will 56 | % neutrulize to 0. 57 | %data.u(t, :) = -10 + 20 * rand(1, config.num_Robots * 2); 58 | 59 | % Let the initial velocity in [0 10] 60 | % PROBLEM: The whole swarm will not move. The swarm velocity will 61 | % neutrulize to 0. 62 | data.u(t, :) = config.u_Ini*2 * rand(1, config.num_Robots * 2)-10; 63 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% 64 | 65 | % region to initialize robots within 66 | xMin = config.siz_Ini(1, 1); 67 | xMax = config.siz_Ini(1, 2); 68 | yMin = config.siz_Ini(2, 1); 69 | yMax = config.siz_Ini(2, 2); 70 | 71 | for i = 1 : config.num_Robots 72 | overlap = 1; 73 | while overlap 74 | x = rand() * (xMax - xMin) + xMin; % x 75 | y = rand() * (yMax - yMin) + yMin; % y 76 | dist = pdist2([x, y], obs(:, 1 : 2)); 77 | if sum(dist' < obs(:, 3) + config.rad_Rob) % at least overlap with one obs 78 | overlap = 1; 79 | else 80 | overlap = 0; 81 | end 82 | end 83 | data.p(t, (config.dim_X * (i - 1) + 1) : (config.dim_X * i)) = [x, y]; 84 | end 85 | 86 | % rng('shuffle'); 87 | % x = rand(config.num_Robots, 1) .* (xMax - xMin) + xMin; 88 | % y = rand(config.num_Robots, 1) .* (yMax - yMin) + yMin; 89 | % initial_rnd_position = [x y]; 90 | % data.p(t, :) = reshape(initial_rnd_position, 1, numel(initial_rnd_position)); 91 | 92 | end -------------------------------------------------------------------------------- /MakeColorMap.m: -------------------------------------------------------------------------------- 1 | function cMap = MakeColorMap(varargin) 2 | %% MAKECOLORMAP makes smoothly varying colormaps 3 | % a = makeColorMap(beginColor, middleColor, endColor, numSteps); 4 | % a = makeColorMap(beginColor, endColor, numSteps); 5 | % a = makeColorMap(beginColor, middleColor, endColor); 6 | % a = makeColorMap(beginColor, endColor); 7 | % 8 | % all colors are specified as RGB triples 9 | % numSteps is a scalar saying howmany points are in the colormap 10 | % 11 | % Examples: 12 | % 13 | % peaks; 14 | % a = makeColorMap([1 0 0],[1 1 1],[0 0 1],40); 15 | % colormap(a) 16 | % colorbar 17 | % 18 | % peaks; 19 | % a = makeColorMap([1 0 0],[0 0 1],40); 20 | % colormap(a) 21 | % colorbar 22 | % 23 | % peaks; 24 | % a = makeColorMap([1 0 0],[1 1 1],[0 0 1]); 25 | % colormap(a) 26 | % colorbar 27 | % 28 | % peaks; 29 | % a = makeColorMap([1 0 0],[0 0 1]); 30 | % colormap(a) 31 | % colorbar 32 | 33 | % Reference: 34 | % A. Light & P.J. Bartlein, "The End of the Rainbow? Color Schemes for 35 | % Improved Data Graphics," Eos,Vol. 85, No. 40, 5 October 2004. 36 | % http://geography.uoregon.edu/datagraphics/EOS/Light&Bartlein_EOS2004.pdf 37 | 38 | defaultNum = 100; 39 | errorMessage = 'See help MAKECOLORMAP for correct input arguments'; 40 | 41 | if nargin == 2 %endPoints of colormap only 42 | color.start = varargin{1}; 43 | color.middle = []; 44 | color.end = varargin{2}; 45 | color.num = defaultNum; 46 | elseif nargin == 4 %endPoints, midPoint, and N defined 47 | color.start = varargin{1}; 48 | color.middle = varargin{2}; 49 | color.end = varargin{3}; 50 | color.num = varargin{4}; 51 | elseif nargin == 3 %endPoints and num OR endpoints and Mid 52 | if numel(varargin{3}) == 3 %color 53 | color.start = varargin{1}; 54 | color.middle = varargin{2}; 55 | color.end = varargin{3}; 56 | color.num = defaultNum; 57 | elseif numel(varargin{3}) == 1 %numPoints 58 | color.start = varargin{1}; 59 | color.middle = []; 60 | color.end = varargin{2}; 61 | color.num = varargin{3}; 62 | else 63 | error(errorMessage) 64 | end 65 | else 66 | error(errorMessage) 67 | end 68 | 69 | if color.num <= 1 70 | cMap = color.end;%error(errorMessage) 71 | end 72 | 73 | if isempty(color.middle) %no midPoint 74 | cMap = interpMap(color.start, color.end, color.num); 75 | else %midpointDefined 76 | [topN, botN] = sizePartialMaps(color.num); 77 | cMapTop = interpMap(color.start, color.middle, topN); 78 | cMapBot = interpMap(color.middle, color.end, botN); 79 | cMap = [cMapTop(1:end-1,:); cMapBot]; 80 | end 81 | 82 | 83 | function cMap = interpMap(colorStart, colorEnd, n) 84 | 85 | for i = 1:3 86 | cMap(1:n,i) = linspace(colorStart(i), colorEnd(i), n); 87 | end 88 | 89 | function [topN, botN] = sizePartialMaps(n) 90 | n = n + 1; 91 | 92 | topN = ceil(n/2); 93 | botN = floor(n/2); 94 | % Copyright 2008 - 2009 The MathWorks, Inc. -------------------------------------------------------------------------------- /Move.m: -------------------------------------------------------------------------------- 1 | function data = Move(t, data, config) 2 | %% --------------------------------------------------% 3 | % --------------------------------------------------- input 4 | % t - current time step 5 | % data - data.p - T x (nR x 3) mat - [x, y] - up to (t-1) 6 | % - data.u - T x (nR x 2) mat - [vx, vy] - up to (t) 7 | % - data.z - T x (nR) cell - n x [id] - up to (t-1) 8 | % config - dim_X/alpha 9 | % --------------------------------------------------- output 10 | % data - data.p - T x (nR x 3) mat - [x, y] - up to (t) 11 | % - data.u - T x (nR x 2) mat - [vx, vy] - up to (t) 12 | % - data.z - T x (nR) cell - n x [id] - up to (t-1) 13 | % --------------------------------------------------- 14 | 15 | for i = 1 : config.num_Robots 16 | x = data.p(t-1, config.dim_X * (i-1) + 1); 17 | y = data.p(t-1, config.dim_X * (i-1) + 2); 18 | vx = data.u(t, 2 * (i-1) + 1); 19 | vy = data.u(t, 2 * (i-1) + 2); 20 | 21 | % keep within range 22 | x = mod(x + vx * config.dt, config.siz_Fig); 23 | y = mod(y + vy * config.dt, config.siz_Fig); 24 | 25 | data.p(t, (config.dim_X * (i-1) + 1) : (config.dim_X * (i))) = [x, y]; 26 | end 27 | 28 | end -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Swarm-Robotic-Formation-Control 2 | =============================== 3 | Bio-inspired Robotics Fall 2013 Course Project 4 | 5 | Author 6 | =============================== 7 | Jingwei~Zhang, Chi~Liu, and~Jia~Zeng 8 | Carnegie~Mellon~University 9 | 10 | Abstract 11 | =============================== 12 | Social insect colonies possess remarkable abilities to select the best among several courses of action. In populous societies with highly efficient recruitment behavior, coordination is distributed across all individuals, each acting on limited local information with appropriate decision rules. 13 | 14 | Nowadays, the development of robotic technology improves the capabilities of robots, and the robotic application fields are expanding all the time. People want the robots to accomplish more complicated tasks which cannot be achieved by single robot, therefore coordination among multi-robot has been put forward and become a hot topic now. Due to limited resources, multi-robot coordination has to solve the problem that how to make the robots cooperate with each other with high efficiency, and the cooperation can result in one of the most important advantages of the multi-mbot system which is the ability of a single robot can be enlarged. Space, as an important factor of multi-robot cooperation, has a large influence on the efficiency of multi-robot coordination. Inspired by the swarm behavior in nature, we build a system that enables multi-robot to form certain types of physical structure that ensures the space occupied by every robot enables the robots optimally formate, avoid collision and prepare them for future tasks. 15 | 16 | In the following sections, related works including the natural origin, swarm robot and behavior based robotics are first introduced. In the next section, problem setup is introduced for the following implementation details. Flocking simulation, formation control and obstacle avoidance details are separately explained then. After the implementation we compare different simulation results and figure out the best parameter set in each formation control. Current limitations and future works are discussed in the end. 17 | 18 | Keywords 19 | =============================== 20 | swarm robotics, behavior, formation, obstacle avoidance. 21 | -------------------------------------------------------------------------------- /Sense.m: -------------------------------------------------------------------------------- 1 | function data = Sense(t, data, obs, config, sensor) 2 | %% --------------------------------------------------% 3 | % --------------------------------------------------- input 4 | % t - current time step 5 | % data - data.p - T x (nR x 3) mat - [x, y] - up to (t) 6 | % - data.u - T x (nR x 2) mat - [vx, vy] - up to (t) 7 | % - data.z - T x (nR) cell - n x [id] - up to (t-1) 8 | % - data.zO- T x (nR) cell - n x [id] - up to (t-1) 9 | % obs - n x [x, y, siz] 10 | % config - num_Robots/ 11 | % sensor - 12 | % --------------------------------------------------- output 13 | % data - data.p - T x (nR x 3) mat - [x, y] - up to (t) 14 | % - data.u - T x (nR x 2) mat - [vx, vy] - up to (t) 15 | % - data.z - T x (nR) cell - n x [id] - up to (t) 16 | % - data.zO- T x (nR) cell - n x [id] - up to (t) 17 | % --------------------------------------------------- 18 | 19 | data.p = real(data.p); 20 | data.u = real(data.u); 21 | 22 | X = data.p(t, 1 : config.dim_X : end-(config.dim_X-1))'; 23 | Y = data.p(t, 2 : config.dim_X : end-(config.dim_X-2))'; 24 | range = pdist2([X, Y], [X, Y]); 25 | 26 | for i = 1 : config.num_Robots 27 | cx = data.p(t, config.dim_X * (i-1) + 1); 28 | cy = data.p(t, config.dim_X * (i-1) + 2); 29 | ctheta = atan2(data.u(t, 2 * (i-1) + 2), data.u(t, 2 * (i-1) + 1)); 30 | 31 | % --------------------------------------------- % other robots 32 | rangeInd = range(:, i) < sensor.range; % ind with valid range 33 | 34 | if abs(sensor.bearing - 2*pi) >= 0.01 35 | bearing = atan2(Y - cy, X - cx) - ctheta; 36 | bearing = StandardizeAngle(bearing); 37 | bearingInd = abs(bearing) < sensor.bearing; % ind with valid bearing 38 | 39 | ind = rangeInd .* bearingInd; 40 | else % if can sense circly, just chech range 41 | ind = rangeInd; 42 | end 43 | 44 | ind(i) = 0; % itself should not be included 45 | z = 1 : config.num_Robots; 46 | data.z{t, i} = z(ind == 1)'; 47 | 48 | % --------------------------------------------- % obs 49 | dist = pdist2([cx, cy], obs(:, 1:2))'; 50 | rangeInd = dist < sensor.range + obs(:, 3); % ind with valid range 51 | 52 | bearing = atan2(obs(:,2)-cy, obs(:,1)-cx) - ctheta; 53 | bearing = StandardizeAngle(bearing); 54 | bearingInd = abs(bearing) < pi/2;%sensor.bearing; % ind with valid bearing 55 | ind = rangeInd .* bearingInd; 56 | data.zO{t, i} = obs(ind==1, :); 57 | end -------------------------------------------------------------------------------- /Simulator.m: -------------------------------------------------------------------------------- 1 | function Simulator() 2 | %% --------------------------------------------------% 3 | % main simulation 4 | % --------------------------------------------------- input 5 | % --------------------------------------------------- output 6 | % --------------------------------------------------- 7 | 8 | %% -------------------------------------------------- set parameters 9 | global config sensor form; 10 | [config, sensor, form] = ConfigFile(); 11 | 12 | fig = figure(1); % for the main simulator 13 | set(fig, 'position', [675 105 700 700]); 14 | hold on; 15 | 16 | % --------------------------------------------------% *movie* 17 | if config.movie == 1 % 18 | winsize = get(fig, 'Position'); % 19 | winsize(1 : 2) = [0 0]; % 20 | numframes = 440; % 21 | A = moviein(numframes, fig, winsize); % 22 | set(fig, 'NextPlot', 'replacechildren'); % 23 | end % 24 | % --------------------------------------------------% 25 | 26 | %% -------------------------------------------------- initialize 27 | t = 1; 28 | 29 | % --------------------------------------------------% *movie* 30 | if config.movie == 1 % 31 | A(:, t) = getframe(fig, winsize); % 32 | end % 33 | % --------------------------------------------------% 34 | 35 | global data obs; 36 | 37 | [data, obs] = Initialize(t, config); 38 | DrawWorld(data.p(t, :), data.u(t, :), obs, config, sensor); 39 | grid on; 40 | 41 | %% -------------------------------------------------- main loop 42 | while t < config.T 43 | if config.pause 44 | disp('--------------------------'); 45 | pause; 46 | else 47 | pause(0.015); 48 | end 49 | t = t + 1; 50 | 51 | % ----------------------------------------------% *movie* 52 | if config.movie % 53 | A(:, t) = getframe(fig, winsize); % 54 | end % 55 | % ----------------------------------------------% 56 | clf; 57 | 58 | [data, form] = Control(t, data, config, sensor, form); 59 | data = Move(t, data, config); 60 | data = Sense(t, data, obs, config, sensor); 61 | DrawWorld(data.p(t, :), data.u(t, :), obs, config, sensor); 62 | grid on; 63 | end 64 | 65 | 66 | %% -------------------------------------------------- save 67 | save log.mat ... 68 | config sensor form ... 69 | data obs... 70 | t 71 | 72 | % --------------------------------------------------% *movie* 73 | if config.movie % 74 | save movie.mat A; % 75 | movie2avi(A, 'movie.avi'); % 76 | end % 77 | % --------------------------------------------------% 78 | 79 | 80 | end -------------------------------------------------------------------------------- /StandardizeAngle.m: -------------------------------------------------------------------------------- 1 | function theta = StandardizeAngle(theta) 2 | %% --------------------------------------------------% 3 | % bring any angle to [-180 180] deg 4 | % --------------------------------------------------- input 5 | % --------------------------------------------------- output 6 | % --------------------------------------------------- 7 | 8 | theta = rem(theta, 2 * pi); 9 | theta(theta > pi) = theta(theta > pi) - 2 * pi; 10 | theta(theta < - pi) = theta(theta < - pi) + 2 * pi; 11 | 12 | end -------------------------------------------------------------------------------- /log.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ferherranz/Swarm-Robotic-Formation-Control/0f09cd8f43ca2fb7a77f3280512aef05f21dd1ce/log.mat -------------------------------------------------------------------------------- /obs.m: -------------------------------------------------------------------------------- 1 | function [du, w] = Obstacle(p, u, o, rad_Rob, u_Max, sensor_Range) 2 | %% --------------------------------------------------% 3 | % Obstacle - obstacle avoidance 4 | % --------------------------------------------------- input 5 | % p - 1 x [x, y] - position of itself 6 | % u - 1 x [vx, vy] - velocity of itself 7 | % o - n x [x, y, siz] - obs detected 8 | % - within sensing range & bearing(only those in front) 9 | % rad_Rob 10 | % u_Max 11 | % sensor_Range 12 | % --------------------------------------------------- output 13 | % du - [dvx, dvy] 14 | % - velo turbulence under this rule 15 | % w - between [0, 1] 16 | % - 1 if < thres 17 | % --------------------------------------------------- 18 | 19 | if isempty(o) 20 | du = [0, 0]; 21 | w = 0; 22 | return; 23 | else 24 | numO = size(o, 1); 25 | du = zeros(numO, 2); 26 | w = zeros(numO, 1); 27 | 28 | for i = 1 : size(o, 1) 29 | theta = atan2(u(2), u(1)); % bearing of the robot 30 | distRO = pdist2(p, o(i, 1 : 2)); % dist between center of r & o 31 | dist = DistPt2Ln_thetaP(o(i, 1 : 2), theta, p); % dist from o center to proceeding line of r 32 | r = o(i, 3) + rad_Rob + 10; % expand o with rad of r, and a redun 10 33 | if abs(dist) < r % possible to collide 34 | % ----------------------------------------- % get len 35 | len = sqrt(distRO^2 - dist^2) - sqrt(r^2 - dist^2); 36 | % ----------------------------------------- % get beta 37 | beta = asin(abs(dist)/r); 38 | if dist > 0 % obs is on the left of the robot 39 | beta = theta - (pi/2 - beta); 40 | else % obs is on the right of the robot 41 | beta = theta + (pi/2 - beta); 42 | end 43 | % hold on; 44 | % DrawBearing(p(1), p(2), beta, 100, 'r', '-'); 45 | % ----------------------------------------- % get du(i, :) & w(i) 46 | % lenMax = sqrt(distRO^2 - r^2); 47 | % lenMin = 0; 48 | lenSafe = norm(u); 49 | du(i, :) = [cos(beta), sin(beta)]; 50 | if len < lenSafe 51 | du(i, :) = [cos(beta), sin(beta)]; 52 | w(i) = 1; 53 | else 54 | du(i, :) = [cos(beta), sin(beta)] - ... 55 | u / norm(u); 56 | du(i, :) = du(i, :) / norm(du(i, :)); 57 | w(i) = (r+sensor_Range-distRO) / (r+sensor_Range); 58 | % (len-lenMax)^2 / (lenSafe-lenMax)^2 * (0.618) + ... 59 | % (r - abs(dist)) / r * (1 - 0.618); 60 | end 61 | else 62 | w(i) = -1; 63 | end 64 | end 65 | 66 | du = du(w~=-1, :); 67 | w = w(w~=-1); 68 | 69 | if isempty(w) 70 | du = [0, 0]; 71 | w = 0; 72 | else 73 | if sum(w==1) 74 | du = mean(du(w==1, :), 1); 75 | w = 1; 76 | else 77 | du = mean(du .* repmat(w ./ sum(w), 1, 2), 1); 78 | w = mean(w); 79 | end 80 | end 81 | end 82 | 83 | end -------------------------------------------------------------------------------- /run.m: -------------------------------------------------------------------------------- 1 | close all; 2 | clear all; 3 | clc; 4 | 5 | % --------------------------------------------------- 6 | % data - data.p - T x (nR x 3) mat - [x, y] - true poses of robots 7 | % - data.u - T x (nR x 2) mat - [vx, vy] - motion control 8 | % - data.z - T x (nR) cell - n x [id] - readings 9 | % - data.zO- T x (nR) cell - n x [x, y, siz] 10 | % @ t, use z(t-1) to get u(t) then p(t) 11 | % --------------------------------------------------- 12 | 13 | warning off; 14 | Simulator(); --------------------------------------------------------------------------------