基于matlab的小车避障仿真

基于matlab对小车的避障功能进行仿真，首先自己设置初始点、目标和障碍点，然后运算出除障碍外的最佳路径

%定义二维地图数组
MAX_X=20;
MAX_Y=20;
MAX_VAL=1;
%此数组存储地图的坐标和每个坐标中的对象
MAP=2*（ones（MAX_XMAX_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（h5）;
if ishandle（h） == 1
    delete（h）;
end
xlabel（‘请用鼠标左键选择目标‘‘Color‘‘green‘）;
but=0;
while （but == 0） %重复，直到左键未被单击为止
    [xvalyvalbut]=ginput（1）;
end
xval=floor（xval）;
yval=floor（yval）;
xTarget=xval;
yTarget=yval;

MAP（xvalyval）=0;%用目标位置初始化映射
plot（xval+.5yval+.5‘gd‘）;
text（xval+1yval+.5‘Target‘）


pause（2）;
h=msgbox（‘用鼠标左键选择障碍物，用鼠标右键选择最后一个障碍物‘）;
  xlabel（‘用鼠标左键选择障碍物，用鼠标右键选择最后一个障碍物‘‘Color‘‘red‘）;
uiwait（h10）;
if ishandle（h） == 1
    delete（h）;
end
while but == 1
    [xvalyvalbut] = ginput（1）;
    xval=floor（xval）;
    yval=floor（yval）;
    MAP（xvalyval）=-1;
    rectangle（‘Position‘[xvalyval11]‘facecolor‘‘black‘）;
    
end



pause（1）;
h=msgbox（‘请用鼠标左键选择车辆初始位置‘）;
uiwait（h5）;
if ishandle（h） == 1
    delete（h）;
end
xlabel（‘请选择车辆初始位置 ‘‘Color‘‘blue‘）;
but=0;
while （but ~= 1） %重复，直到左键未被单击为止
    [xvalyvalbut]=ginput（1）;
end
xval=floor（xval）;
yval=floor（yval）;
xStart=xval;%起始位置
yStart=yval;%起始位置
MAP（xvalyval）=1;
 plot（xval+.5yval+.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_VAL2）;
CLOSED=[];

%把所有的障碍都列在封闭的名单上
k=1;
for i=1:MAX_X
    for j=1:MAX_Y
        if（MAP（ij） == -1）
            CLOSED（k1）=i; 
            CLOSED（k2）=j; 
            k=k+1;
        end
    end
end
CLOSED_COUNT=size（CLOSED1）;
%将起始节点设置为第一个节点
xNode=xval;
yNode=yval;
OPEN_COUNT=1;
path_cost=0;
goal_distance=distance（xNodeyNodexTargetyTarget）;
OPEN（OPEN_COUNT:）=insert_open（xNodeyNodexNodeyNodepath_costgoal_distancegoal_distance）;%目标坐标，原坐标，所需路径，移动前距离，移动后距离
OPEN（OPEN_COUNT1）=0;
CLOSED_COUNT=CLOSED_COUNT+1;
CLOSED（CLOSED_COUNT1）=xNode;
CLOSED（CLOSED_COUNT2）=yNode;
NoPath=1;


%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%启动算法
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
while（（xNode ~= xTarget || yNode ~= yTarget） && NoPath == 1）
%  plot（xNode+.5yNode+.5‘go‘）;
 exp_array=expand_array（xNodeyNodepath_costxTargetyTargetCLOSEDMAX_XMAX_Y

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----
     文件        6823  2019-04-22 13:11  bizhang\bizhang.m
     文件         201  2010-01-05 14:23  bizhang\distance.m
     文件        1690  2019-04-12 10:00  bizhang\expand_array.m
     文件         400  2019-04-09 09:42  bizhang\insert_open.m
     文件         889  2019-04-09 17:48  bizhang\min_fn.m
     文件        3637  2019-04-13 23:22  bizhang\move.m
     文件         208  2019-04-09 22:45  bizhang\node_index.m
     目录           0  2019-04-22 13:12  bizhang\