├── DADM_test.mlx ├── LICENSE ├── README.md ├── STC_main.m ├── STC_test.mlx ├── charge field.fig ├── combine two.fig ├── display_area.m ├── divide_area.m ├── divide_area_v2.m ├── divide_area_v3.m ├── divide_area_v4.m ├── divide_area_v5.m ├── function design.png ├── gauss.fig ├── isconnect.m ├── main.m ├── result.fig └── test.mlx /DADM_test.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Wei-Fan/multi-robot_coverage_planning/57027f8ddcf0ca79d10d3d8f3bf6499bcacfc806/DADM_test.mlx -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2019 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # multi-robot_coverage_planning 2 | DARP+STC algorithm for mCPP problem 3 | -------------------------------------------------------------------------------- /STC_main.m: -------------------------------------------------------------------------------- 1 | Test for STC path generation 2 | 3 | preparation 4 | LENGTH = 20; 5 | 6 | rng(30,'twister'); 7 | A_core = unifrnd(0,1,[LENGTH/2,LENGTH/2]); 8 | den = 0.05; 9 | A_core(A_core>den) = 1;% freee space 10 | A_core(A_coreLENGTH/2||tem_p(2)<=0||tem_p(2)>LENGTH/2 56 | continue; 57 | end 58 | if K(tem_p(2),tem_p(1))==1 59 | line([tem_p(1),start_p(1)],[tem_p(2),start_p(2)],'Color','r','LineWidth',2) 60 | curr_p = tem_p; 61 | num = num + 1; 62 | T(tem_p(2),tem_p(1)) = num; 63 | break; 64 | end 65 | end 66 | 67 | while num~=Total_num 68 | %num 69 | % move to free space 70 | move = 0; 71 | for d=dir 72 | tem_p = curr_p + d; 73 | if tem_p(1)<=0||tem_p(1)>LENGTH/2||tem_p(2)<=0||tem_p(2)>LENGTH/2 74 | continue; 75 | end 76 | if K(tem_p(2),tem_p(1))==1 && T(tem_p(2),tem_p(1))==0 77 | line([tem_p(1),curr_p(1)],[tem_p(2),curr_p(2)],'Color','r','LineWidth',2) 78 | curr_p = tem_p; 79 | num = num + 1; 80 | T(tem_p(2),tem_p(1)) = num; 81 | move = 1; 82 | break; 83 | end 84 | end 85 | % back to an old cell 86 | if move==1 87 | continue; 88 | end 89 | for d=dir 90 | tem_p = curr_p + d; 91 | if tem_p(1)<=0||tem_p(1)>LENGTH/2||tem_p(2)<=0||tem_p(2)>LENGTH/2 92 | continue; 93 | end 94 | if K(tem_p(2),tem_p(1))==1 && T(tem_p(2),tem_p(1))==T(curr_p(2),curr_p(1))-1 95 | curr_p = tem_p; 96 | break; 97 | end 98 | end 99 | end 100 | 101 | 102 | 103 | 104 | -------------------------------------------------------------------------------- /STC_test.mlx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Wei-Fan/multi-robot_coverage_planning/57027f8ddcf0ca79d10d3d8f3bf6499bcacfc806/STC_test.mlx -------------------------------------------------------------------------------- /charge field.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Wei-Fan/multi-robot_coverage_planning/57027f8ddcf0ca79d10d3d8f3bf6499bcacfc806/charge field.fig -------------------------------------------------------------------------------- /combine two.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Wei-Fan/multi-robot_coverage_planning/57027f8ddcf0ca79d10d3d8f3bf6499bcacfc806/combine two.fig -------------------------------------------------------------------------------- /display_area.m: -------------------------------------------------------------------------------- 1 | function [ handler] = display_area( Area, object_num, figure_num ) 2 | % Area:L*W (L:length of SOI, W:width of SOI, Area(i,j)=1 means free space/0 means obstacles) 3 | %% draw the grid map 4 | %figure(figure_num); 5 | B = Area; 6 | B(end+1,end+1) = 0; 7 | if object_num == 1 8 | colormap([0 0 0;1 1 1]); % color 9 | else 10 | colormap([0 0 0;unifrnd(0,1,[object_num,3])]); 11 | end 12 | pcolor(0.5:size(Area,2)+0.5,0.5:size(Area,1)+0.5,B); % grid color 13 | set(gca,'XTick',1:size(Area,2),'YTIck',1:size(Area,1)); % set coordinate 14 | %axis image xy; 15 | handler = gcf; 16 | name = ['subarea_',num2str(figure_num)]; 17 | set(handler,'Name',name); 18 | 19 | end 20 | 21 | -------------------------------------------------------------------------------- /divide_area.m: -------------------------------------------------------------------------------- 1 | function [ Area_rst ] = divide_area( Core_flag, init_grid, robot_num ) 2 | % Area_flag:L*W (L:length of SOI, W:width of SOI, Area_flag(i,j)=1 means free space/0 means obstacles) 3 | % init_pos:robot_num*2 4 | % Area_rst:L*W*robot_num (Area_rst(:,:,i) means the area for robot i) 5 | 6 | %% prepare control points 7 | Core_flag = Core_flag-1; %Area_flag(i,j)=0 means free space/-1 means obstacles 8 | L = size(Core_flag,2); %length = cols = x 9 | W = size(Core_flag,1); %width = rols = y 10 | %if mod(L,2)~=0 || mod(W,2)~=0 11 | % Area_rst = -1; 12 | % fprintf('area error\n') 13 | % return 14 | %end 15 | F = L*W; 16 | m = ones(1,robot_num); 17 | E = zeros(W,L,robot_num); 18 | K = zeros(W,L); 19 | D = ones(1,robot_num)*(-1); %keep track of connectivity radius 20 | D_vec = zeros(robot_num,2); 21 | C = ones(W,L,robot_num); %modified matrix 22 | penality_theshold = 3; 23 | %S = zeros(1,robot_num); 24 | 25 | %display_area(Area_flag,1); 26 | 27 | stop = 0; 28 | isfirst = 1; 29 | while stop==0 30 | S = zeros(1,robot_num); 31 | ob = 0; 32 | count = 0; 33 | %% check every grid 34 | for i=1:L 35 | for j=1:W 36 | if Core_flag(j,i)==-1 37 | ob = ob + 1; 38 | continue; 39 | end 40 | 41 | for k=1:robot_num 42 | %if m(k)<0 43 | % fprintf('error'); 44 | %end 45 | E(j,i,k) = C(j,i,k) * m(k) * sqrt((i-init_grid(k,1))^2+(j-init_grid(k,2))^2); 46 | end 47 | [~,index] = min(E(j,i,:)); 48 | K(j,i) = index; 49 | S(index) = S(index)+1; 50 | rst_t = sqrt((i-init_grid(index,1))^2+(j-init_grid(index,2))^2); 51 | if isfirst == 1 52 | if D(index) < rst_t 53 | D(index) = rst_t; 54 | D_vec(index,:) = [i-init_grid(index,1),j-init_grid(index,2)]; 55 | end 56 | else 57 | c = D_vec(index,:)*[i-init_grid(index,1);j-init_grid(index,2)]; 58 | d = rst_t - c/rst_t; 59 | if d > penality_theshold 60 | C(j,i,index) = C(j,i,index) * d; 61 | elseif d > 0 62 | D(index) = rst_t; 63 | end 64 | end 65 | end 66 | end 67 | isfirst = 0; 68 | 69 | %% update m 70 | %dm = zeros(1,robot_num); 71 | dm = S - (F-ob)/robot_num; 72 | for k=1:robot_num 73 | if dm(k) > 1.8 || dm(k) < -1.8 74 | m(k) = m(k) + 0.002 * dm(k); 75 | else 76 | count = count + 1; 77 | end 78 | end 79 | %for i=1:L 80 | % for j=1:W 81 | % if Area_src(j,i)~=1 82 | % continue; 83 | % end 84 | % if dm() <= 1 || dm >= -1 85 | % m(j,i) = m(j,i) - 0.2 * dm(); 86 | % count = count + 1; 87 | % end 88 | % end 89 | %end 90 | if count == robot_num 91 | stop = 1; 92 | end 93 | S 94 | %m 95 | %dm 96 | %count 97 | Area_rst = K; 98 | figure(2); 99 | handle2 = display_area(Area_rst,robot_num,2); 100 | end 101 | %m 102 | %S 103 | %K = K/10; 104 | %colormap([0 0 0;rand(robot_num,3)]); 105 | %pcolor(1:L,1:W,K); 106 | 107 | 108 | end 109 | 110 | -------------------------------------------------------------------------------- /divide_area_v2.m: -------------------------------------------------------------------------------- 1 | function [ output_args ] = divide_area_v2( Core_flag, init_grid, robot_num ) 2 | % Area_flag:L*W (L:length of SOI, W:width of SOI, Area_flag(i,j)=1 means free space/0 means obstacles) 3 | % init_pos:robot_num*2 4 | % Area_rst:L*W*robot_num (Area_rst(:,:,i) means the area for robot i) 5 | %i-x,j-y 6 | 7 | %% prepare control points 8 | Core_flag = Core_flag-1; %Area_flag(i,j)=0 means free space/-1 means obstacles 9 | L = size(Core_flag,2); %length = cols = x 10 | W = size(Core_flag,1); %width = rols = y 11 | %if mod(L,2)~=0 || mod(W,2)~=0 12 | % Area_rst = -1; 13 | % fprintf('area error\n') 14 | % return 15 | %end 16 | F = L*W; 17 | m = ones(1,robot_num); 18 | E = zeros(W,L,robot_num); 19 | K = zeros(W,L); %assignment matrix 20 | C = ones(W,L,robot_num); %modified matrix 21 | %penality_theshold = 3; 22 | %S = zeros(1,robot_num); 23 | 24 | %display_area(Area_flag,1); 25 | 26 | stop = 0; 27 | isfirst = 1; 28 | while stop==0 29 | S = zeros(1,robot_num); %number of grids for each region 30 | D = ones(1,robot_num)*(-1); %keep track of largest grid distance 31 | D_vec = zeros(robot_num,2); %vector of the D 32 | ob = 0; 33 | count = 0; 34 | R_vec = zeros(robot_num,2);%region vector 35 | R_svec = zeros(robot_num,2);%region square vector 36 | %% check every grid 37 | % assignment each grid and calculate region parameters 38 | for i=1:L 39 | for j=1:W 40 | if Core_flag(j,i)==-1 41 | ob = ob + 1; 42 | continue; 43 | end 44 | 45 | for k=1:robot_num 46 | %if m(k)<0 47 | % fprintf('error'); 48 | %end 49 | E(j,i,k) = C(j,i,k) * m(k) * sqrt((i-init_grid(k,1))^2+(j-init_grid(k,2))^2); 50 | end 51 | [~,index] = min(E(j,i,:)); 52 | K(j,i) = index; 53 | S(index) = S(index)+1; 54 | R_vec(index,:) = R_vec(index,:) + [j,i] - init_grid(index,:); 55 | R_svec(index,:) = R_svec(index,:) + ([j,i] - init_grid(index,:)).^2; 56 | 57 | rst_t = sqrt((i-init_grid(index,1))^2+(j-init_grid(index,2))^2); 58 | if D(index) < rst_t 59 | D(index) = rst_t; 60 | D_vec(index,:) = [i-init_grid(index,1),j-init_grid(index,2)]; 61 | end 62 | end 63 | end 64 | % finish grid assignment 65 | R_vec = R_vec ./ [S; S]'; 66 | R_sigma = sqrt(R_svec ./ [S; S]') 67 | 68 | 69 | %% update modified 70 | h = @(v,ind) exp(-sum(((v - R_vec(ind,:))./R_sigma(ind,:)).^2) / 2); % Guass 71 | q1 = 2; q2 = 1.5; 72 | g = @(v,ind) q1/(sum(v - init_grid(ind,:)))^0.4 + q2/(sum(v - init_grid(ind,:) - R_vec(ind,:)))^0.4; % charge like field 73 | % update through each grid 74 | for i=1:L 75 | for j=1:W 76 | if Core_flag(j,i)==-1 77 | ob = ob + 1; 78 | continue; 79 | end 80 | index = K(j,i); 81 | 82 | % update modified matrix -- input: 83 | % init_grid,(x,y),D,D_vec,R_vec,R_sigma 84 | % output: C 85 | for ind = 1:robot_num 86 | the = g(D_vec(ind,:)*q2/(q1+q2)+init_grid(ind,:),ind); 87 | z1 = h([j,i],ind); 88 | z2 = g([j,i],ind); 89 | if z2 > the 90 | z2 = 1; 91 | else 92 | z2 = z2 / the; 93 | end 94 | z = z1 * z2; 95 | if z > 0.25 96 | z = 1; 97 | else 98 | z = z / 0.25; 99 | end 100 | C(j,i,ind) = z; 101 | end 102 | end 103 | end 104 | 105 | %% update m 106 | %dm = zeros(1,robot_num); 107 | dm = S - (F-ob)/robot_num; 108 | for k=1:robot_num 109 | if dm(k) > 2 || dm(k) < -2 110 | m(k) = m(k) + 0.002 * dm(k); 111 | else 112 | count = count + 1; 113 | end 114 | end 115 | 116 | %% stop condition 117 | if count == robot_num 118 | stop = 1; 119 | end 120 | S 121 | %C(:,:,1) 122 | %m 123 | %dm 124 | %count 125 | Area_rst = K; 126 | figure(2); 127 | handle2 = display_area(Area_rst,robot_num,2); 128 | end 129 | %m 130 | %S 131 | %K = K/10; 132 | %colormap([0 0 0;rand(robot_num,3)]); 133 | %pcolor(1:L,1:W,K); 134 | 135 | 136 | end 137 | 138 | 139 | -------------------------------------------------------------------------------- /divide_area_v3.m: -------------------------------------------------------------------------------- 1 | function [ Area_rst ] = divide_area_v3( Core_flag, init_grid, robot_num ) 2 | % Area_flag:L*W (L:length of SOI, W:width of SOI, Area_flag(i,j)=1 means free space/0 means obstacles) 3 | % init_pos:robot_num*2 4 | % Area_rst:L*W*robot_num (Area_rst(:,:,i) means the area for robot i) 5 | 6 | %% prepare control points 7 | Core_flag = Core_flag-1; %Area_flag(i,j)=0 means free space/-1 means obstacles 8 | L = size(Core_flag,2); %length = cols = x 9 | W = size(Core_flag,1); %width = rols = y 10 | %if mod(L,2)~=0 || mod(W,2)~=0 11 | % Area_rst = -1; 12 | % fprintf('area error\n') 13 | % return 14 | %end 15 | F = L*W; 16 | m = ones(1,robot_num); 17 | E = zeros(W,L,robot_num); 18 | K = zeros(W,L); 19 | coef = ones(1,robot_num)*0.002; 20 | 21 | %S = zeros(1,robot_num); 22 | 23 | %display_area(Area_flag,1); 24 | 25 | stop = 0; 26 | isfirst = 1; 27 | while stop==0 28 | S = zeros(1,robot_num); 29 | K0 = zeros(W,L); %temporary assignment matrix 30 | acc = zeros(1,robot_num); 31 | ob = 0; 32 | count = 0; 33 | %% check every grid 34 | for i=1:L 35 | for j=1:W 36 | if Core_flag(j,i)==-1 37 | ob = ob + 1; 38 | continue; 39 | end 40 | 41 | for k=1:robot_num 42 | %if m(k)<0 43 | % fprintf('error'); 44 | %end 45 | E(j,i,k) = m(k) * sqrt((i-init_grid(k,1))^2+(j-init_grid(k,2))^2); 46 | end 47 | [~,index] = min(E(j,i,:)); 48 | if isfirst == 1 49 | K0(j,i) = index; 50 | else 51 | go = 0; 52 | if j>=2 53 | if K0(j-1,i) == index 54 | go = go+1; 55 | end 56 | end 57 | if i>=2 58 | if K0(j,i-1) == index 59 | go = go+1; 60 | end 61 | end 62 | if j<=W-1 63 | if K0(j+1,i) == index 64 | go = go+1; 65 | end 66 | end 67 | if i<=L-1 68 | if K0(j,i+1) == index 69 | go = go+1; 70 | end 71 | end 72 | if go >= 2 73 | K0(j,i) = index; 74 | else 75 | K0(j,i) = K(j,i); 76 | acc(index) = acc(index) + 1; 77 | index = K(j,i); 78 | 79 | end 80 | end 81 | S(index) = S(index)+1; 82 | end 83 | end 84 | isfirst = 0; 85 | K = K0; 86 | 87 | %% update m 88 | %dm = zeros(1,robot_num); 89 | dm = S - (F-ob)/robot_num; 90 | for k=1:robot_num 91 | if dm(k) > 2 || dm(k) < -2 92 | coef(k) = coef(k) + acc(k)*0.0002; 93 | m(k) = m(k) + coef(k) * dm(k); 94 | else 95 | count = count + 1; 96 | end 97 | end 98 | 99 | 100 | if count == robot_num 101 | stop = 1; 102 | end 103 | S 104 | m 105 | coef 106 | %dm 107 | %count 108 | Area_rst = K; 109 | figure(2); 110 | handle2 = display_area(Area_rst,robot_num,2); 111 | end 112 | %m 113 | %S 114 | %K = K/10; 115 | %colormap([0 0 0;rand(robot_num,3)]); 116 | %pcolor(1:L,1:W,K); 117 | 118 | 119 | end 120 | 121 | -------------------------------------------------------------------------------- /divide_area_v4.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Wei-Fan/multi-robot_coverage_planning/57027f8ddcf0ca79d10d3d8f3bf6499bcacfc806/divide_area_v4.m -------------------------------------------------------------------------------- /divide_area_v5.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Wei-Fan/multi-robot_coverage_planning/57027f8ddcf0ca79d10d3d8f3bf6499bcacfc806/divide_area_v5.m -------------------------------------------------------------------------------- /function design.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Wei-Fan/multi-robot_coverage_planning/57027f8ddcf0ca79d10d3d8f3bf6499bcacfc806/function design.png -------------------------------------------------------------------------------- /gauss.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Wei-Fan/multi-robot_coverage_planning/57027f8ddcf0ca79d10d3d8f3bf6499bcacfc806/gauss.fig -------------------------------------------------------------------------------- /isconnect.m: -------------------------------------------------------------------------------- 1 | function [ bool ] = isconnect( j0,i0,K,init_robot ) 2 | % j-W,i-L 3 | % K=1 represents assigned,0 represents unassigned 4 | % remeber the iteration journey and increase the cost of turning back 5 | pace = 0; 6 | coordinate = [0 1 0 -1;-1 0 1 0];% 1-up 2-right 3-down 4-left 7 | robot = zeros(2,1); 8 | robot(1) = init_robot(2); 9 | robot(2) = init_robot(1); 10 | curr_p = [j0;i0]; 11 | 12 | stop = 0; 13 | count = 0; 14 | while stop==0 15 | %% see if the condition were met 16 | curr_p; 17 | dir = robot - curr_p; 18 | if dir(1)==0 && dir(2)==0 19 | bool = 1; 20 | stop = 1; 21 | continue; 22 | end 23 | dir = dir/sqrt(sum(dir.^2)); 24 | 25 | dir_cost = sqrt(sum((coordinate - dir).^2)); 26 | if pace~=0 27 | dir_cost(pace) = dir_cost(pace) + 2; 28 | end 29 | [~,dir_i] = sort(dir_cost); 30 | sorted_cd = zeros(2,4); 31 | for i=1:4 32 | sorted_cd(:,i) = coordinate(:,dir_i(i)); 33 | end 34 | %dir_cost 35 | %sorted_cd 36 | %% choose the nearest approachable way 37 | move = 0; 38 | for sd=sorted_cd 39 | next_p = zeros(2,1); 40 | next_p = curr_p + sd; 41 | if next_p(1)==0||next_p(1)==size(K,1)+1||next_p(2)==0||next_p(2)==size(K,2)+1 42 | continue; 43 | end 44 | if K(next_p(1),next_p(2))==1 45 | curr_p = next_p; 46 | move = 1; 47 | if sd(1)==1 48 | pace = 4; 49 | elseif sd(1)==-1 50 | pace = 2; 51 | elseif sd(2)==-1 52 | pace = 3; 53 | else 54 | pace = 1; 55 | end 56 | break; 57 | end 58 | end 59 | if move==0 60 | bool = 0; 61 | stop = 1; 62 | else 63 | count = count+1; 64 | if count==size(K,1)*size(K,2) 65 | bool = 0; 66 | stop = 1; 67 | end 68 | end 69 | end 70 | 71 | end 72 | -------------------------------------------------------------------------------- /main.m: -------------------------------------------------------------------------------- 1 | ROBOT_NUM = 4; 2 | LENGTH = 20; %must be even 3 | 4 | rng(40 ,'twister');%24 17 5 | %4-20: 36 32 21 20? 18? 16 15 9? 8? 6 | %4-40: 8? 9? 15? 16 20? 21? 32? 36 7 | 8 | %% generate matrix 9 | %core = unifrnd(0,1,[LENGTH/2,LENGTH/2]); 10 | %den = 0.05; 11 | %core(core>den) = 1; 12 | %core(core