image thumbnail
from Ultrasonic Sensor Robot by Ligong Han
simulation of environment modeling for mobile robot based on ultrasonic sensor

rs=sonomeasure(segArray,rpos,polygon,ns)
function rs=sonomeasure(segArray,rpos,polygon,ns)
rx=rpos(1);
ry=rpos(2);
n=size(segArray,1);
if nargin<4
    ns=24;
    if nargin<3
        polygon=false;
    end
end
if polygon
    segArray=[segArray(1:end-1,:),segArray(2:end)];
end
a=2*pi/ns;%angle between two adjacent sensors
% Simulate the ultrasonic data
% Measure: Point Discretization %to be improved
dl=.1;
segadd=[];
for k=1:n
    lx=segArray(k,1)-segArray(k,3);
    ly=segArray(k,2)-segArray(k,4);
    l=sqrt(lx^2+ly^2);
    if lx
        xadd=segArray(k,1):-(dl*lx/l):segArray(k,3);
        yadd=(xadd-segArray(k,1))*ly/lx+segArray(k,2);
    else
        yadd=segArray(k,2):-(dl*ly/l):segArray(k,4);
        xadd=segArray(k,1)*ones(1,length(yadd));
    end
    segadd=[segadd;[xadd',yadd']];
end
segadd=[segadd;segArray(:,1:2);segArray(:,3:4)];

%determine sections
nadd=size(segadd,1);
nsect=zeros(1,nadd);
ssect=zeros(ns,nadd+1);
for k=1:nadd
    ak=vectorangle([1,0],segadd(k,:)-[rx,ry],1)/a;
    nsect(k)=ceil(ak)+(ak==0);
    sk=nsect(k);
    ssect(sk,1)=ssect(sk,1)+1;
    ssect(sk,ssect(sk,1)+1)=k;
end
%choose min
rs=zeros(1,ns);%distance radius of probing
for k=1:ns
    if ssect(k,1)
        pts=segadd(ssect(k,2:1+ssect(k,1)),:);
        rs(k)=min((pts(:,1)-rx).^2+(pts(:,2)-ry).^2);rs(k)=sqrt(rs(k));
    else
        rs(k)=0;%inf;
    end
end
end

Contact us