Small Satellites

Home » 2014 » January

Monthly Archives: January 2014

Distance Transform Path Planning Algorithm

January 26, 2014 11:24 am / 2 Comments on Distance Transform Path Planning Algorithm

clc;close all;clear all;

Input Map

RGB = imread('Mapn.png');
figure('Position',[600 0 600 1000],'color','k');
image(RGB);
hold on;
axis off
axis image
hold off;
% Convert to binary image matrix and inverse matrix values
I = rgb2gray(RGB);
binaryImage = im2bw(RGB, 0.3);
binaryImage = 1-binaryImage;
% Inital pose
xs = 540; ys = 750;
% Goal Pose
xg = 150; yg = 150;

dist_01

Distance to Goal

sz  = size(binaryImage);
for i = 1:sz(1)
    for j = 1:sz(2)
       rg(i,j) = sqrt((i-yg)^2 +(j - xg)^2);
      end
end
% Pose Matrix
pose = zeros(sz(1),sz(2));
pose(ys,xs) = 1;
pose(yg,xg) = 1;
Di = 15; % Distance of influence of the obstacles
% Compute for every pixel the distance to the nearest obstacle(non zero
% elements which after inversion corespondent to the obstacles)
[D,IDX] = bwdist(binaryImage);
for i = 1:sz(1)
    for j = 1:sz(2)
        rd = D(i,j);
        if (rd <= Di)
            rg(i,j)  = 1e6;
        end
    end
end

Distance Transform Path Planning Algorithm

figure('Position',[600 0 600 1000],'color','k');
hold on;
image(RGB);
colormap gray
contour(rg,15);
spy(pose,'*r');
axis off
axis image
title('Distance Transform Path Planning Algorithm','color','w');
x = xs; y = ys;
last = rg(y-1,x-1);
while (x ~= xg)||(y ~= yg)
    dis =[ rg(y-1,x-1), rg(y-1,x),rg(y-1,x+1);
        rg(y,x-1), rg(y,x) ,rg(y,x+1);
        rg(y+1,x-1), rg(y+1,x),rg(y+1,x+1)];
     m = min(dis(:));
     [r,c] = find(dis == m);
   rg(y,x) = 1E6;
   y = y-2+r;
   x = x-2+c;
   pose(y,x) = 1;
  end
spy(pose,'.r');
set(gcf, 'InvertHardCopy', 'off');
hold off;

dist_02

Voronoi Road Map, Path Planing

January 26, 2014 11:17 am / Leave a comment

clc;close all;clear all;

Input Map

RGB = imread('Map.png');
figure('Position',[100 0 600 700],'color','k');
imagesc(RGB);
axis image
title('Input Map','color','w');
axis off
set(gcf, 'InvertHardCopy', 'off');
hold off;

voron_01

Convert to binary image matrix and inverse matrix values

I = rgb2gray(RGB);
binaryImage = im2bw(RGB, 0.3);
sz  = size(binaryImage);

Inital pose

xs = 225; ys = 355;
% Goal Pose
xg = 50; yg = 50;
% Pose Matrix
pose = zeros(sz(1),sz(2));
pose(ys,xs) = 1;
pose(yg,xg) = 1;

Voronoi Road Map

figure('Position',[100 0 600 700],'color','k');
hold on;
set(gcf, 'InvertHardCopy', 'off');
sk = 1;
for i = 1:sz(1)
    for j = 1:sz(2)
         if(binaryImage(i,j) == 1 )
     xv(sk) = j;
     yv(sk) = i;
     sk = sk+1;
         end
    end
end
[vrx,vry]  = voronoi(xv,yv);
set(gca,'YDir','Reverse');
plot(xv,yv,'y.',vrx,vry,'b-');
axis tight
%axis([1 sz(2) 1 sz(1) ]) ;
axis image
title({'Voronoi Road Map';
'* - Represents Starting and Goal Position '},'color','w');
axis off
spy(pose,'*r');

voron_02

Robot Path

x = xs; y = ys;
indk = 1;
while (abs(x - xg) > 5)||(abs(y - yg)> 5)
path(:,indk) = [x y];
dist = sqrt((vrx - x).^2+(vry - y).^2);
goal = sqrt((xg - x).^2+(xg - y).^2);
% Find closest vertices of the Voronoi edges
[mn ind] = min(dist(:));
xt = vrx(ind);
yt = vry(ind);
goalj = sqrt((xg - xt).^2+(xg - yt).^2);
if (goalj < goal )
x = xt;
y = yt;
vrx(ind) = 1E6;
vry(ind) = 1E6;
indk = indk +1;
else
vrx(ind) = 1E6;
vry(ind) = 1E6;
end
end
path(:,indk) = [xg yg];
plot(path(1,:),path(2,:),'-r');
title({'Voronoi Road Map and Robot Path';
'* - Represents Starting and Goal Position '},'color','w');

voron_03

 

Potential Field Path Planning

January 26, 2014 11:11 am / 9 Comments on Potential Field Path Planning

clc;close all;clear all;

Input Map

RGB = imread('Mapn.png');
figure('Position',[600 0 600 1000],'color','k');
image(RGB);
hold on;
axis off
axis image

potential_01

Convert to binary image matrix and inverse matrix values

I = rgb2gray(RGB);
binaryImage = im2bw(RGB, 0.3);
binaryImage = 1-binaryImage;
% Inital pose
xs = 540; ys = 750;
% Goal Pose
xg = 150; yg = 150;

Attractive potential

sz  = size(binaryImage);
for i = 1:sz(1)
    for j = 1:sz(2)
       rg = sqrt((i-yg)^2 +(j - xg)^2);
       U_att(i,j) = 0.5*rg^2;
    end
end
contour(U_att,40);

set(gcf, 'InvertHardCopy', 'off');
% Pose Matrix
pose = zeros(sz(1),sz(2));
pose(ys,xs) = 1;
pose(yg,xg) = 1;
spy(pose,'*r');
title({'Attractive Potential Field';...
   'Circles Center Corespondents to the Goal Position '},'color','w');
hold off;

potential_02

Repulsive potential

Di = 35; % Distance of influence of the object
% Compute for every pixel the distance to the nearest obstacle(non zero
% elements which after inversion corespondent to the obstacles)
[D,IDX] = bwdist(binaryImage);
for i = 1:sz(1)
    for j = 1:sz(2)
        rd = D(i,j);
        if (rd <= Di)
            U_rep(i,j)  = 0.5*(rd - Di)^2;
        else
            U_rep(i,j)  = 0;
        end
    end
end
figure('Position',[600 0 600 1000],'color','k');
hold on;
colormap jet;
contourf(U_rep,5);
spy(pose,'*r');
axis off
axis image
set(gcf, 'InvertHardCopy', 'off');
title('Repulsive Potential Field','color','w');
hold off;

potential_03

Summary Potential Field

k_s  = 2; % Scaling factor
U_sum = U_rep/max(U_rep(:))+k_s*U_att/max(U_att(:));
figure('Position',[600 0 600 1000],'color','k');
hold on;
contourf(U_sum,15);
spy(pose,'*r');
axis off
axis image
title('Summary Potential Field','color','w');
set(gcf, 'InvertHardCopy', 'off');
hold off;

potential_04

Potential Field Path Planning

figure('Position',[600 0 600 1000],'color','k');
hold on;
contourf(U_sum,15);
spy(pose,'*r');
axis off
axis image
title('Potential Field Path Planning,*Note the Potential Trap','color','w');
x = xs; y = ys;
last = U_sum(y-1,x-1);
while (x ~= xg)||(y ~= yg)
    dis =[ U_sum(y-1,x-1), U_sum(y-1,x),U_sum(y-1,x+1);
        U_sum(y,x-1), U_sum(y,x) ,U_sum(y,x+1);
        U_sum(y+1,x-1), U_sum(y+1,x),U_sum(y+1,x+1)];
     m = min(dis(:));
     [r,c] = find(dis == m);
   U_sum(y,x) = inf;
   y = y-2+r;
   x = x-2+c;
   pose(y,x) = 1;
  end
spy(pose,'.r');
set(gcf, 'InvertHardCopy', 'off');
hold off;

potential_05

 

Computing a Skeleton of a Map for Mobile Robots,Path Planning

January 13, 2014 4:28 pm / Leave a comment

clc;close all;clear all;

Input Map

RGB = imread('Map1.png');
figure('Position',[0 0 800 800]);
imshow(RGB);


Robot Map1


Convert to binary image matrix and inverse matrix values


I = rgb2gray(RGB);
binaryImage = im2bw(RGB, 0.3);
binaryImage = 1-binaryImage;
figure('Position',[0 0 800 800]);
imshow(binaryImage);


Warning: Image is too big to fit on screen; displaying at
50%


Robot Map 1


Compute for every pixel the distance to the nearest obstacle(non zero elements which after inversion corespondent to the obstacles)


[D,IDX] = bwdist(binaryImage);
figure('Position',[0 0 800 800],'color','k')
colormap bone
image(D);
axis off          % Remove axis ticks and numbers
axis image
set(gcf, 'InvertHardCopy', 'off');


Robot Map 3


Taking the Laplacian of the distance transform. Play with the treshold value 0.15 to remove or add more futures in skeleton


lap = del2(D);
sz  = size(lap);
for i = 1:sz(1)
    for j = 1:sz(2)
         if( abs(lap(i,j)) < 0.15)
       lap(i,j) = 0;
    end
    end
end
lap = flipdim(lap ,1);           %# vertical flip
figure('Position',[0 0 800 800],'color','k');
colormap winter;
contour(lap,'DisplayName','lap');
axis off
axis image
set(gcf, 'InvertHardCopy', 'off');


Robot Skeleton




 









	
Swirl,Matlab Code

	

		January 3, 2014 4:48 pm / Leave a comment	

	
 




clc; clear all; close all;
x = -20:0.1:20;
sz = size(x);
ind = 0;
b = 6
for n = 1:15 %
for i = 1:sz(2)
    for j = 1:sz(2)
        sw(i,j) = sin(b*cos(sqrt(x(i)^2+x(j)^2))-n*atan2(x(i),x(j)));
    end
end
ind = ind +1;
fig = figure('Position',[0 0 800 800]);
hold on;
imagesc(sw);
colormap bone;
axis off;
set(fig, 'color', [0 0 0]);
set(gcf, 'InvertHardCopy', 'off');
hold off;
%print(fig,['Sw',num2str(ind)],'-djpeg  ','-r300');
close all;
end














	

		Post navigation	

	← Older posts	 	




	




			
		


				
		




	

		

	

				
		

		
		



	
	


			
		
		
	
				





		
				
			
	

		

			%d bloggers like this: