Path_AstarB.rar

Path_AstarB.rar

% Astar algorithm of route planning
% Copyright（c） 2009-2014 by Chenglong Xu All rights reserved.
% Northeastern University Shenyang China
% 2018/6/6 14:36:03
function Path_AstarB
clc;
clear;
close all;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%读入地图
img=imread（‘2.jpg‘）;
[rowscolschannel]=size（img）;
for i=1:rows
    for j=1:cols
      if img（ij1）<=40&&img（ij2）>=230&&img（ij3）<=40%Start点
       StartP（11）=i; StartP（12）=j;
      end
      if 230<=img（ij1）&&img（ij2）<=40&&img（ij3）<=40%End点
       EndP（11）=i; EndP（12）=j;
      end
    end
end
thresh=graythresh（img）;
im=im2bw（imgthresh）;%01二维地图
map = ones（rowscols）; %img为二值化矩阵，不能直接改变需定义中间map矩阵
ow=0.5;%颜色中心
for i=1:rows
    for j=1:cols
      if im（ij）==0
         map（ij）=2;
      end
    end
end
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%自定义地图
% rows = 20;%图大小
% cols = 30;
% map = ones（rowscols）; 
% % 设置障障碍 
% map（8:164:8） = 2; 
% map（10:1822:26） = 2;
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%%  set up color map for display 
%       R/255 G/255 B/255
cmap = [0.8 1 1;    % 1 - white - clear cell 第一个位置 colormap1颜色
        0 0 0;      % 2 - black - obstacle 障碍
        1 0 0;      % 3 - red -  end  终点
        0 0 1;      % 4 - blue - new-delete 新的点但是又被淘汰的
        0 1 0;      % 5 - green - start 起点
        0.5 0 0.5;  % 6 - Popul – 
        1 0 1;      % 7 - Pitch – lastpath 最后规划的轨迹
        1 1 0];     % 8 - Yellow – history 历史搜索方向
colormap（cmap）;
start_r=StartP（11）;
start_c=StartP（12）;
end_r=EndP（11）;
end_c=EndP（12）;
map（start_rstart_c） = 5; % 设置起点颜色 
map（end_rend_c） = 3; % 设置终点颜色
%ShowMap（owmapcolsrows）;
%获取始点、终点索引
start_node = sub2ind（size（map） start_r start_c）; %将元素map（start_r start_c）的下标转换成线性索引号 列搜索
end_node = sub2ind（size（map） end_r end_c）; 
% 探索矩阵初始化
CheckPath = Inf（rowscols）; 
CheckPath（start_node） = 0; 
%定义H函数
[X Y] = meshgrid （1:1:cols 1:1:rows）;
H = abs（Y - end_r） + abs（X - end_c）;%H 曼哈顿距离
f = Inf（rowscols）; 
f（start_node） = H（start_node）;
% 对于每个网格单元，这个数组保存其父节点的索引。 
parent=zeros（rowscols）; %父节点索引
num=0;
Depth=0;%移动步数
%%主循环
%%
t0=cputime;%开始计时
while 1 %不断迭代
 num=num+1;%搜索次数
      fprintf（‘【INFO:--------------算法运行次数：%d】\n‘num）;
 %更新状态图
 map（start_node）= 5; 
 map（end_node） = 3; 
% ShowMap（owmapcolsrows）;
  [~ current] = min（f（:））; %返回当前f最小值的索引
  [min_dist ~] = min（CheckPath（:））; %返回当前距离数组所有数的最小值。min（CheckPath）为按列返回最小值
  if （（current == end_node） || isinf（min_dist）） %搜索到目标点或者全部搜索完，结束循环
    scrsz = get（0‘ScreenSize‘）;  % 获取屏幕分辨率
    set（gcf‘Position‘[scrsz（1） scrsz（2） ceil（cols/50*scrsz（3）/1.5） ceil（rows/30*scrsz（4）/1.5）]）; 
     t1=cputime-t0;%计算时间
     fprintf（‘【INFO:--------------运行时间:%3.3fs 】\n‘t1）;
     fprintf（‘【INFO:--------------路径规划完成！   】\n‘）;
       break; 
  end; 
map（current） = 8; %将当前颜色标为黄色
f（current） = Inf;  %当前区域在距离数组中设置为无穷，表示已搜索
 [ij] = ind2sub（size（CheckPath） current）; %返回当前位置的坐标
neighbo

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件      11837  2018-06-05 22:16  Path_AstarB\1.jpg

     文件      13124  2018-06-05 22:51  Path_AstarB\2.jpg

     文件      15302  2018-06-05 23:18  Path_AstarB\3.jpg

     文件      41879  2018-06-06 00:02  Path_AstarB\4.jpg

     文件      42116  2018-06-05 23:56  Path_AstarB\5.jpg

     文件      41757  2018-06-06 00:11  Path_AstarB\6.jpg

     文件     561666  2018-06-06 01:23  Path_AstarB\7.jpg

     文件       5424  2018-06-06 02:10  Path_AstarB\Path_AstarB.m

     文件     579158  2018-06-06 01:32  Path_AstarB\捕获.JPG

     文件     143555  2018-06-06 02:11  Path_AstarB\捕获2.JPG

     目录          0  2018-06-06 02:12  Path_AstarB

----------- ---------  ---------- -----  ----

              1455818                    11