基于matlab的小车避障仿真

%定义二维地图数组 MAX_X=20; MAX_Y=20; MAX_VAL=1; %此数组存储地图的坐标和每个坐标中的对象 MAP=2*(ones(MAX_X,MAX_Y)); %获取障碍物、目标和机器人位置初始化地图，输入障碍物为-1、目标为0、机器人为1、空间为2 j=0; x_val = 1; y_val = 1; axis equal axis([1 MAX_X+1 1 MAX_Y+1]) %用来标注输出的图线的最大值最小值 set(gca,'XTick',1:1:21); set(gca,'YTick',1:1:21); grid on;%显示表格 hold on;%当前轴及图像保持而不被刷新，准备接受此后将绘制的图形，多图共存 n=0;%障碍物数量 %开始交互障碍物、目标、开始位置选择 pause(1); h=msgbox('请用鼠标左键选择目标'); uiwait(h,5); if ishandle(h) == 1 delete(h); end xlabel('请用鼠标左键选择目标','Color','green'); but=0; while (but == 0) %重复，直到左键未被单击为止 [xval,yval,but]=ginput(1); end xval=floor(xval); yval=floor(yval); xTarget=xval; yTarget=yval; MAP(xval,yval)=0;%用目标位置初始化映射 plot(xval+.5,yval+.5,'gd'); text(xval+1,yval+.5,'Target') pause(2); h=msgbox('用鼠标左键选择障碍物，用鼠标右键选择最后一个障碍物'); xlabel('用鼠标左键选择障碍物，用鼠标右键选择最后一个障碍物','Color','red'); uiwait(h,10); if ishandle(h) == 1 delete(h); end while but == 1 [xval,yval,but] = ginput(1); xval=floor(xval); yval=floor(yval); MAP(xval,yval)=-1; rectangle('Position',[xval,yval,1,1],'facecolor','black'); end pause(1); h=msgbox('请用鼠标左键选择车辆初始位置'); uiwait(h,5); if ishandle(h) == 1 delete(h); end xlabel('请选择车辆初始位置 ','Color','blue'); but=0; while (but ~= 1) %重复，直到左键未被单击为止 [xval,yval,but]=ginput(1); end xval=floor(xval); yval=floor(yval); xStart=xval;%起始位置 yStart=yval;%起始位置 MAP(xval,yval)=1; plot(xval+.5,yval+.5,'bo'); %障碍物目标拾取结束 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %用于算法的列表 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %开放列表结构 %-------------------------------------------------------------------------- %IS ON LIST 1/0 |X val |Y val |Parent X val |Parent Y val |h(n) |g(n)|f(n)| %-------------------------------------------------------------------------- OPEN=[]; %封闭列表结构 %-------------- %|X val | Y val | %-------------- % CLOSED=zeros(MAX_VAL,2); CLOSED=[]; %把所有的障碍都列在封闭的名单上 k=1; for i=1:MAX_X for j=1:MAX_Y if(MAP(i,j) == -1) CLOSED(k,1)=i; CLOSED(k,2)=j; k=k+1; end end end CLOSED_COUNT=size(CLOSED,1); %将起始节点设置为第一个节点 xNode=xval; yNode=yval; OPEN_COUNT=1; path_cost=0; goal_distance=distance(xNode,yNode,xTarget,yTarget); OPEN(OPEN_COUNT,:)=insert_open(xNode,yNode,xNode,yNode,path_cost,goal_distance,goal_distance);%目标坐标，原坐标，所需路径，移动前距离，移动后距离 OPEN(OPEN_COUNT,1)=0; CLOSED_COUNT=CLOSED_COUNT+1; CLOSED(CLOSED_COUNT,1)=xNode; CLOSED(CLOSED_COUNT,2)=yNode; NoPath=1; %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %启动算法 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% while((xNode ~= xTarget || yNode ~= yTarget) && NoPath == 1) % plot(xNode+.5,yNode+.5,'go'); exp_array=expand_array(xNode,yNode,path_cost,xTarget,yTarget,CLOSED,MAX_X,MAX_Y);%障碍外的坐标 exp_count=size(exp_array,1); %使用后续节点更新开放列表 %开放列表格式 %-------------------------------------------------------------------------- %IS ON LIST 1/0 |X val |Y val |Parent X val |Parent Y val |h(n) |g(n)|f(n)| %-------------------------------------------------------------------------- %扩展数组格式 %-------------------------------- %|X val |Y val ||h(n) |g(n)|f(n)| %-------------------------------- %查找所有可行的路径 for i=1:exp_count flag=0; for j=1:OPEN_COUNT if(exp_array(i,1) == OPEN(j,2) && exp_array(i,2) == OPEN(j,3) ) OPEN(j,8)=min(OPEN(j,8),exp_array(i,5)); %#ok<*SAGROW> if OPEN(j,8)== exp_array(i,5) OPEN(j,4)=xNode; OPEN(j,5)=yNode; OPEN(j,6)=exp_array(i,3); OPEN(j,7)=exp_array(i,4); end%最小FN检查结束 flag=1; end % if flag == 1 % break; end%End of j for if flag == 0 OPEN_COUNT = OPEN_COUNT+1; OPEN(OPEN_COUNT,:)=insert_open(exp_array(i,1),exp_array(i,2),xNode,yNode,exp_array(i,3),exp_array(i,4),exp_array(i,5)); end%在开放列表中插入新元素的结束 end %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% %找出具有最小fn的节点 index_min_node = min_fn(OPEN,OPEN_COUNT,xTarget,yTarget); if (index_min_node ~= -1) %将xnode和ynode设置为最小fn的节点 xNode=OPEN(index_min_node,2); yNode=OPEN(index_min_node,3); path_cost=OPEN(index_min_node,6);%更新到达原节点的成本 %将节点移动到封闭列表 CLOSED_COUNT=CLOSED_COUNT+1; CLOSED(CLOSED_COUNT,1)=xNode; CLOSED(CLOSED_COUNT,2)=yNode; OPEN(index_min_node,1)=0; else %!目标没有路径！！ NoPath=0; end end %一旦算法运行，通过在最后一个节点开始（如果它是目标节点）， %然后识别其父节点，直到它到达起始节点，生成最佳路径。这是最佳路径。 i=size(CLOSED,1); Optimal_path=[]; xval=CLOSED(i,1); yval=CLOSED(i,2); i=1; Optimal_path(i,1)=xval; Optimal_path(i,2)=yval; i=i+1; if ( (xval == xTarget) && (yval == yTarget)) inode=0; %遍历开放列表并确定原节点 parent_x=OPEN(node_index(OPEN,xval,yval),4);% node_index返回节点的索引 parent_y=OPEN(node_index(OPEN,xval,yval),5); while( parent_x ~= xStart || parent_y ~= yStart) Optimal_path(i,1) = parent_x; Optimal_path(i,2) = parent_y; %Get the grandparents:-) inode=node_index(OPEN,parent_x,parent_y); parent_x=OPEN(inode,4);% parent_y=OPEN(inode,5); i=i+1; end Optimal_path(i,1) = parent_x; Optimal_path(i,2) = parent_y; inode=node_index(OPEN,parent_x,parent_y); parent_x=OPEN(inode,4); parent_y=OPEN(inode,5); %绘制最佳路径! j=size(Optimal_path,1); p=plot(Optimal_path(j,1)+.5,Optimal_path(j,2)+.5,'bo'); j=j-1; for i=j:-1:1 pause(.25); set(p,'XData',Optimal_path(i,1)+.5,'YData',Optimal_path(i,2)+.5); drawnow ; end Optimal_path=Optimal_path'; a=[]; b=[]; a=Optimal_path(1,:); b=Optimal_path(2,:); values = spcrv([[a(1) a a(end)];[b(1) b b(end)]],4); %subplot(2,2,4); plot(values(1,:)+.5,values(2,:)+.5,'r'); axis([1,21,1,21]); end