matlaborientationrobotics

How to measure the distance for robot center to obstacles?


I want to orient my robot based on the distance from the obstacle, but I do not know how to measure the distance from robot center to the obstacle surrounding.

enter image description here


Solution

  • 1.- GET/BUILD AN ACCURATE MAP

    Let's start binarizing the supplied may and dropping a point bot on a random position excluding all obstacle points.

    close all;clear all;clc
    
    % get the map
    A1=imread('001.jpg');  % this is your map in question
    % figure;imshow(A1)
    A2=~imbinarize(A1(:,:,1));
    figure(1);
    ax1=gca;imshow(A2)
    

    enter image description here

    [ny,nx]=size(A2);
    [X,Y]=meshgrid(1:nx,1:ny);
    
    % obstacles
    P1=[140 311; 153 311; 153 7; 140 7; 140 311];          % 140 311 153 7    
    P2=[374 491; 386 491; 386 186; 374 186; 374 491];      %  374 491 386 186
    P3=[1 497; 496 497; 496 492; 1 492; 1 497];   %  1 497 496 492
    P4=[1 6; 496 6; 496 1; 1 1; 1 6];   % 6 1 496 1
    P5=[1 491; 3 491; 3 7; 1 7; 1 491];  % left frame 1 491  3 7
    P6=[492 491; 496 491; 496 7; 492 7; 492 491];  % right frame  %  492 491 
    Pobst=[P1(:);P2(:);P3(:);P4(:);P5(:);P6(:)];
    Nobst=6;
    Po
    
    bst=reshape(Pobst,[size(P1,1),size(P1,2),Nobst]);
    
    % map points where bot cannot be 
    [in1,on1]=inpolygon(X(:),Y(:),P1(:,1),P1(:,2));
    [in2,on2]=inpolygon(X(:),Y(:),P2(:,1),P2(:,2));
    [in3,on3]=inpolygon(X(:),Y(:),P3(:,1),P3(:,2));
    Xout=X(~in1 & ~in2 & ~in3);Yout=Y(~in1 & ~in2 & ~in3);
    
    % dropping bot on random location, excluding obstacle points
    nP2=randi([1 numel(Xout)],1,1);
    Pnet=[Xout(nP2) Yout(nP2)];
    % Pnet=[47 365];  % static test point
    
    hold(ax1,'on')
    plot(ax1,Pnet(1),Pnet(2),'or','LineStyle','none');
    

    enter image description here

    2.- 360º SWEEP

    Now let's do a 360º ray sweep.

    NOT using angles BUT directly covering all outer perimeter points, from wherever the bot is located.

    Ns=2*size(A2,1)+2*size(A2,2)-4;  % amount outer perimeter points 
    
    % outer perimeter defined starting top left corner and then CW
    P0=[ones(1,size(A2,1))  2:1:size(A2,2)-1                       size(A2,2)*ones(1,size(A2,1))    size(A2,2)-1:-1:2;          % x
           1:1:size(A2,1)        size(A2,1)*ones(1,size(A2,2)-2) size(A2,1):-1:1                         ones(1,size(A2,2)-2)]';    % y
    
    PR=[0 0];
    for k1=1:10:Ns
        L2=floor(linspace(Pnet(2),P0(k1,1),max([Pnet(1) Pnet(2) abs(P0(k1,2)-Pnet(2)) abs(P0(k1,1)-Pnet(1)) ]) ));   % x
        L1=floor(linspace(Pnet(1),P0(k1,2),max([Pnet(1) Pnet(2) abs(P0(k1,2)-Pnet(2)) abs(P0(k1,1)-Pnet(1)) ]) ));   % y
       
        p1=1;
        while A2(L2(p1),L1(p1))==0
            p1=p1+1;
        end
        PR=[PR;L1(p1) L2(p1)];   % logging perimeter point [x y]
               
    end
    PR(1,:)=[];
    

    Variable PR contains the points you are asking for, for a particular bot position Pnet

    Quick check all rays and nearest obstacle points are ok

    for k2=1:10:Ns
        plot(ax1,PR(k2,1),PR(k2,2),'r*')
        plot(ax1,[Pnet(1) PR(k2,1)],[Pnet(2) PR(k2,2)],'g-') 
    end
    

    enter image description here

    enter image description here

    3.- Comment on Obtained Perimeter

    At this point, for a particular bot position Pnet the variable perimeter PR contains all the closest obstacle points.

    As here done, the perimeter obstacle points in PR are the 1st point of/belonging to the hard obstacle that the boot should avoid.

    Therefore the bot should NOT consider this point but the 1st one available, thius is, 1 pixel closer to the bot, along each ray, that is NOT part of any obstacle.

    4.- Do not use the bot centre point to know when/where/how to avoid obstacles

    When working out obstacle avoidance the centre of the bot is hardly ever of any use. Yes you can use it for some calculations but the chances that just considering the bot a a point your bot is going to hit things.

    My suggestion is to :

    4.1.- find the furthermost point of the robot, not the centre of the robot

    4.2.- assume the bot as a ball with radius that furthest point

    4.3.- and then include an additional safety area around the ball, not the point that is your bot.

    Yes again, you may actually want the bot to hit a surface, but if you do not control when your bot is not hitting things there's no way you are going to be able to know where/when/how to have it hitting any surface of choice ay time soon.