%?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