MATLAB地图障碍检测

Question

clear all; close all; clc
%function m=Map;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%  Start @ location (12,12). Apply controls to reach (0,0)  %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
xw=-15; xe=15; ys=-15; yn=15; %map dimensions, west, east, south, north
mrx=0.05; mry=0.05; % map resolution {x,y}
[xm ym]=meshgrid(xw:mrx:xe,ys:mry:yn);

m=0*xm.*ym;  % initialize the map m

for i=1:size(xm,2)
    for j=1:size(ym,1)
        r=sqrt(xm(1,i)^2+ym(j,1)^2);
        ang=atan2(ym(j,1),xm(1,i))+pi;
        if 11<=r && r<=12 && (pi/15<=ang && ang<=29*pi/15)
            m(j,i)=1;
        elseif  8<=r && r<=9 && ~(pi-pi/15<=ang && ang<=pi+pi/15)
            m(j,i)=1;
        elseif  5<=r && r<=6 && ~(pi/2+pi-pi/15<=ang && ang<=pi/2+pi+pi/15)
           m(j,i)=1;
        elseif  2<=r && r<=3 && ~(pi/2-pi/9<=ang && ang<=pi/2+pi/9)
            m(j,i)=1;
        elseif xm(1,i)<0.98*xw || 0.98*xe<xm(1,i) || ym(j,1)<0.98*ys || 0.98*yn<ym(j,1) % Boundaries
            m(j,i)=1;
        end
    end
end

mesh(xm,ym,m);    axis image;  view(0, 90); xlabel('x'); ylabel('y');
hold on;
%%
x=12; y=12; th=0; % initial state 
r=1; % r is robot radius
dT=0.05; 
%dS = 0.05;

t1=[0:dT:2]; 
m1=[0*ones(size(t1));(pi/4)*ones(size(t1))];
t2=[0:dT:1];
m2=[1*ones(size(t2)); 0*ones(size(t2))];
t3=[0:dT:1.94];
m3=[0*ones(size(t3)); (pi/4)*ones(size(t3))];
t4=[0:dT:4.5];                            
m4=[3*ones(size(t4)); 0*ones(size(t4))];
t5=[0:dT:0.9];                             
m5=[7.5*pi*ones(size(t5)); pi/2*ones(size(t5))];
t6=[0:dT:1.1];
m6=[0*ones(size(t6)); (pi/2)*ones(size(t6))];
t7=[0:dT:4.5];
m7=[1*ones(size(t7)); 0*ones(size(t7))];
t8=[0:dT:1];
m8=[0*ones(size(t8)); -(pi/2)*ones(size(t8))];
t9=[0:dT:0.9];
m9=[10*pi*ones(size(t9)); (pi)*ones(size(t9))];
t10=[0:dT:1];
m10=[0*ones(size(t10)); (pi/2)*ones(size(t10))];
t11=[0:dT:1];                            
m11=[1.5*ones(size(t11)); 0*ones(size(t11))];
t12=[0:dT:0.95];                            
m12=[0*ones(size(t12)); -(pi/2)*ones(size(t12))];
t13=[0:dT:0.95];                             
m13=[3.5*pi*ones(size(t13)); (pi/2)*ones(size(t13))];
t14=[0:dT:0.95];                            
m14=[0*ones(size(t14)); (pi/2)*ones(size(t14))];
t15=[0:dT:1];                            
m15=[1.8*ones(size(t15)); 0*ones(size(t15))];
t16=[0:dT:0.95];
m16=[0*ones(size(t16)); (pi/2)*ones(size(t16))];
t17=[0:dT:0.95];                             
m17=[4*pi*ones(size(t17)); -pi*ones(size(t17))];
t18=[0:dT:0.95];
m18=[0*ones(size(t18)); -(pi/2)*ones(size(t18))];
t19=[0:dT:1.6];                            
m19=[3*ones(size(t19)); 0*ones(size(t19))];
u = [m1 m2 m3 m4 m5 m6 m7 m8 m9 m10 m11 m12 m13 m14 m15 m16 m17 m18 m19];

%%
for i=1:size(u,2)
    if u(2,i)==0         % to avoid division by zero later
        u(2,i)=100*eps;  % "relatively small" number, but not "too small"
    end
end

for i=1:size(u,2)   
    v=u(1,i); w=u(2,i);
    x=x-v/w*sin(th)+v/w*sin(th+w*dT);
    y=y+v/w*cos(th)-v/w*cos(th+w*dT);
    th=th+w*dT;
    plot(x,y,'ro','MarkerSize',5,'LineWidth',2); hold on;   % DRAWS ROBOTs CIRCLE
    plot([x; 1*r*cos(th)+x], [y; 1*r*sin(th)+y],'w-','LineWidth',2);hold on; % DRAWS BEARING
    axis(15*[-1 1 -1 1]); 
    drawnow; 
    hold off;
    mesh(xm,ym,m);    axis image;  view(0, 90); xlabel('x'); ylabel('y');
    hold on;
end
% 
 plot(0,0,'ro','MarkerSize',5,'LineWidth',2); hold on;   % DRAWS ROBOTs CIRCLE
 plot([0; 1*r*cos(th)], [0; 1*r*sin(th)],'w-','LineWidth',2);hold on; % DRAWS BEARING

这是我上个任务的代码，已经完成了机器人的路径规划，现在要实现地图障碍的检测，达到下面这个效果

 

Answer 1

你好，我是有问必答小助手。为了技术专家团更好地为您解答问题，烦请您补充下（1）问题背景详情，（2）您想解决的具体问题，（3）问题相关代码图片或者报错信息。便于技术专家团更好地理解问题，并给出解决方案。

您可以点击问题下方的【编辑】，进行补充修改问题。