shange.rar_matlab 栅格地图_shange地图_栅格化 matlab_栅格地图_栅格地图 matlab

m = 30;n = 30; M=rand(30,30);%地图矩阵M 0为通路 1为障碍 M(1,1)=0; M(30,30)=0; for i=1:30 for j=1:30 if M(i,j)<=0.65%G中白：黑=0.63:0.35 M(i,j)=0; else M(i,j)=1; end end end for u=1:29 for uu=1:29 if M(u,uu+1)==1&&M(u+1,uu)==1 M(u+1,uu+1)=1; M(u+1,uu)=0; else continue end end end gl=size(M,1); %栅格化 化黑白格 for i=1:gl for j=1:gl if M(i,j)==1 x1=j-1;y1=gl-i; x2=j;y2=gl-i; x3=j;y3=gl-i+1; x4=j-1;y4=gl-i+1; fill([x1,x2,x3,x4],[y1,y2,y3,y4],[0.2,0.2,0.2]); hold on else x1=j-1;y1=gl-i; x2=j;y2=gl-i; x3=j;y3=gl-i+1; x4=j-1;y4=gl-i+1; fill([x1,x2,x3,x4],[y1,y2,y3,y4],[1,1,1]); hold on end end end % % Spoint = []; %起始点坐标 % Epoint = []; %目标点坐标 % % axis([0 m+3 0 n+3]); % % %鼠标确定起点和终点 % [x,y] = ginput(1); % Spoint(1) = round(x); %起始点坐标 % Spoint(2) = round(y); % % [x,y] = ginput(1); % Epoint(1) = round(x); %目标点坐标 % Epoint(2) = round(y); % % detax=Epoint(1)-Spoint(1); % detay=Epoint(2)-Spoint(2); % % if(detax>=0) % flag=1;%终点在起点右边 % else % flag=0;%终点在起点左边 % end % % 显示地图 % %subplot(2,2,1); % h1 = plot(Spoint(1),Spoint(2),'gO'); % hold on % h2 = plot(Epoint(1),Epoint(2),'rO'); % % plot(Epoint(1),Epoint(2),'b+');%画终点 % % plot(Spoint(1),Spoint(2),'b+');%画起点 % hold on; % % % % %%寻路 % M(Spoint(1),Spoint(2))=0; % M(Epoint(1),Epoint(2))=inf; % G=M;%路径G % F=M;%F值矩阵 % openlist=M;%开放列表 待检查列表 % closelist=M;%封闭列表 无需检查 % parentx=M;%上一轮的“下一点”是本轮新加入节点的父节点 % parenty=M;%y坐标 % openlist(Spoint(1),Spoint(2)) =0;%起始点 % % % %路径排序 具有最小F值 % while(1) % num=inf; % for p=1:m+2 % for q=1:n+2 % if(openlist(p,q)==0&&closelist(p,q)~=1)%此时的[p,q]是可通行的 % Outpoint=[p,q]; % if(F(p,q)>=0&&num>F(p,q))%当前点的F值为正数，寻找最小F值，具有最小F值的点为下一点 % num=F(p,q); % Nextpoint=[p,q]; % end % end % end % end % % closelist(Nextpoint(1),Nextpoint(2))=1;%下一点无需被检查 % % for i = 1:3 % for j = 1:3 % k = G(Nextpoint(1)-2+i,Nextpoint(2)-2+j);%k中存放以下一点为中心的9个点的G值 % %更新这周围8个点的F值 % if(i==2&&j==2|closelist(Nextpoint(1)-2+i,Nextpoint(2)-2+j)==1) % continue; % elseif (k == -inf)%遇到障碍 将该点放入封闭列表中 % G(Nextpoint(1)-2+i,Nextpoint(2)-2+j) = G(Nextpoint(1)-2+i,Nextpoint(2)-2+j); % closelist(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=1; % elseif (k == inf)%遇到可走路径 计算距离，更新G值，放入待检查列表中，更新H值 % distance=((i-2)^2+(j-2)^2)^0.5; % G(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=G(Nextpoint(1),Nextpoint(2))+distance; % openlist(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=0; % % H=((Nextpoint(1)-2+i-Epoint(1))^2+(Nextpoint(2)-2+j-Epoint(2))^2)^0.5;%欧几里德距离启发函数 % H_diagonal=min(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比较复杂的对角线启发函数 求得最小H值 % H_straight=abs(Nextpoint(1)-2+i-Epoint(1))+abs(Nextpoint(2)-2+j-Epoint(2));%下一点离终点的直线距离 % H=sqrt(2)*H_diagonal+(H_straight-2*H_diagonal); % % % % H=max(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比较简单的对角线函数 % % F(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=G(Nextpoint(1)-2+i,Nextpoint(2)-2+j)+H;%更新F值 % parentx(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(1);%下一点成为再一下点的父节点 % parenty(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(2); % % else distance=((i-2)^2+(j-2)^2)^0.5;%最开始的时候？？？ % if(k>(distance+G(Nextpoint(1),Nextpoint(2)))) % k=distance+G(Nextpoint(1),Nextpoint(2)); % % H=((Nextpoint(1)-2+i-Epoint(1))^2+(Nextpoint(2)-2+j-Epoint(2))^2)^0.5; %欧几里德距离启发函数 % H_diagonal=min(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比较复杂的对角线启发函数 % H_straight=abs(Nextpoint(1)-2+i-Epoint(1))+abs(Nextpoint(2)-2+j-Epoint(2)); % H=sqrt(2)*10*H_diagonal+10*(H_straight-2*H_diagonal); % % % % H=max(abs(Nextpoint(1)-2+i-Epoint(1)),abs(Nextpoint(2)-2+j-Epoint(2)));%比较简单的对角线函数 % % F(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=k+H; % parentx(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(1); % parenty(Nextpoint(1)-2+i,Nextpoint(2)-2+j)=Nextpoint(2); % end % end % % % if(((Nextpoint(1)-2+i)==Epoint(1)&&(Nextpoint(2)-2+j)==Epoint(2))|num==inf)%如果在本列下一点是终点或者可通行的路 % parentx(Epoint(1),Epoint(2))=Nextpoint(1); % parenty(Epoint(1),Epoint(2))=Nextpoint(2); % break; % end % end % % if(((Nextpoint(1)-2+i)==Epoint(1)&&(Nextpoint(2)-2+j)==Epoint(2))|num==inf)%如果在本行下一点是终点或者可通行的路 % parentx(Epoint(1),Epoint(2))=Nextpoint(1); % parenty(Epoint(1),Epoint(2))=Nextpoint(2); % break; % end % end % if(((Nextpoint(1)-2+i)==Epoint(1)&&(Nextpoint(2)-2+j)==Epoint(2))|num==inf)%走到终点 % parentx(Epoint(1),Epoint(2))=Nextpoint(1); % parenty(Epoint(1),Epoint(2))=Nextpoint(2); % break; % end % end % P=[];%创建一个空矩阵P % s=1; % while(1) % if(num==inf) % break; % end % %subplot(3,1,1); % h4 = plot(Epoint(1),Epoint(2),'b+');%画路径 这里是最开始的时候 % % P(s,:)=Epoint; % s=s+1; % % pause(1); % xx=Epoint(1); % Epoint(1)=parentx(Epoint(1),Epoint(2));%更新终点 % Epoint(2)=parenty(xx,Epoint(2)); % if(parentx(Epoint(1),Epoint(2))==Spoint(1)&&parenty(Epoint(1),Epoint(2))==Spoint(2)) % %subplot(3,1,1); % plot(Epoint(1),Epoint(2),'b+');%画起点后的第一个点 % P(s,:)=Epoint; % break; % end % end % P(s+1,:)=Spoint; % legend([h1,h2,h3,h4],'起始点','目标点','障碍物','移动路线'); % % % count=0;%统计总共走了多少步 % % for i=2:12 % % for j=2:12 % % if(G(i,j)~=inf&&G(i,j)~=-inf) % % count=count+1; % % end % % end % % end % % count; % % P=P'; % a=[]; % b=[]; % a=P(1,:); % b=P(2,:); % figure % subplot(2,1,1); % plot(a,b);%单独画出折线曲线 % axis([0,n+3,0,n+3]); % title('折线图The Path'); % % values = spcrv([[a(1) a a(end)];[b(1) b b(end)]],3);%将得到的折现曲线拟合成光滑的曲线 % %figure % subplot(2,1,2); % plot(values(1,:),values(2,:),'r'); % axis([0,m+3,0,m+3]); % title('光滑曲线图The Smoothing Path'); % % Q=P'; % [pathy,pathx]=size(Q); % % pathy % % pathx % angle=linspace(0,0,pathy);%航向角 % x=linspace(0,0,pathy); % y=linspace(0,0,pathy); % % % for i=1:pathy % x(i)=Q(i); % y(i)=Q(i+pathy); % end % % for i=1:1:(pathy-1) % %a=1 % if(x(i)-x(i+1)>0) % if(y(i)-y(i+1)>0) % angle(i)=45; % elseif(y(i)-y(i+1)==0) % angle(i)=0; % else % angle(i)=-45; % end % elseif(x(i)-x(i+1)==0) % if(y(i)-y(i+1)>0) % angle(i)=90; % else % angle(i)=-90; % end % elseif(x(i)-x(i+1