2021-04-15 14:34

# MATLAB地图障碍检测

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


• 点赞
• 写回答
• 关注问题
• 收藏
• 复制链接分享
• 邀请回答

#### 1条回答

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

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

点赞 评论 复制链接分享