A*演算法Matlab演示程式碼
演示效果：
程式碼：
% 參考文獻1《A星演算法詳解（個人認為最詳細，最通俗易懂的一個版本）》https：//blog。csdn。net/hitwhylz/article/details/23089415
% 參考文獻2《A*演算法中啟發函式H的選取》https：//www。jianshu。com/p/63e68b7daade
clear
，
clc
，
close
all
global
start_point
end_point
current_point
ValueSet
row
=
32
；
clm
=
32
；
NewPoint
=
@（
a
，
b
）
struct
（
‘x’
，
a
，
‘y’
，
b
）；
SetKeyStr
=
@（
x
，
y
）
string
（
num2str
（［
x
，
y
］，
‘%03d’
））；
InvalidKeyStr
=
“
000000
”
；
NewSearchPoint
=
@（
a
，
b
）
struct
（
‘key’
，
SetKeyStr
（
a
，
b
），
‘x’
，
a
，
‘y’
，
b
，
‘G’
，
0
，
‘H’
，
0
，
‘F’
，
0
，
‘parent_key’
，
InvalidKeyStr
）；
HType
=
3
；
%啟發式搜尋代價函式型別
% 各類值設定（同樣用於染色）
ValueSet
=
struct
（
‘passable’
，
255
，
‘impassable’
，
0
，
‘openlist’
，
180
，
‘closelist’
，
120
，
‘start’
，
60
，
‘target’
，
60
，
‘current’
，
30
，
‘path’
，
60
）；
% map中的值設定
map
=
ones
（
row
，
clm
）
*
ValueSet
。
passable
；
［
map
（
1
，
：），
map
（
end
，
：），
map
（：，
1
），
map
（：，
end
）］
=
deal
（
ValueSet
。
impassable
）；
［
map
（
5
，
1
：
10
），
map
（
1
：
6
，
18
），
map
（
10
，
9
：
24
），
map
（
13
：
17
，
13
），
map
（
15
，
20
：
end
），
map
（
17
：
20
，
5
），
map
（
20
，
5
：
20
），
map
（
24
，
1
：
15
），
map
（
23
：
25
，
20
），
map
（
28
，
18
：
27
）］
=
deal
（
ValueSet
。
impassable
）；
［
start_point
，
end_point
］
=
deal
（
NewPoint
（
3
，
3
），
NewPoint
（
30
，
23
））；
%起始/結束點座標
［
map
（
start_point
。
x
，
start_point
。
y
），
map
（
end_point
。
x
，
end_point
。
y
）］
=
deal
（
ValueSet
。
start
，
ValueSet
。
target
）；
% 搜尋點（周圍8個）分別為dx dy 以及移動代價（實際為sqrt（dx*dx+dy*dy））
SearchDxy
=
［
-
1
，
-
1
，
sqrt
（
2
）；
0
，
-
1
，
1
；
1
，
-
1
，
sqrt
（
2
）；
-
1
，
0
，
1
；
1
，
0
，
1
；
-
1
，
1
，
sqrt
（
2
）；
0
，
1
，
1
；
1
，
1
，
sqrt
（
2
）］；
% 1。起始點加入open_list
open_list
（
1
）
=
NewSearchPoint
（
start_point
。
x
，
start_point
。
y
）；
% 初始狀態設定
open_list
（
1
）。
H
=
CalcH
（
start_point
。
x
，
start_point
。
y
，
end_point
。
x
，
end_point
。
y
，
HType
）；
open_list
（
1
）。
F
=
open_list
。
H
；
open_list
=
struct2table
（
open_list
）；
%待確定代價的點
close_list
=
［］；
%已確定代價的點
current_point
=
open_list
；
%當前點（設定為初始點）
figure
b_find_path
=
0
；
% 是否發現路徑
while
~
isempty
（
open_list
）
%2。遍歷open_list，找到最小合代價F點
index_min_open_list_F
=
SearchOptimalPoint
（
open_list
，
close_list
，
current_point
，
end_point
）；
current_point
=
open_list
（
index_min_open_list_F
，
：）；
%最小代價F的點選中為當前點，進行後續open_list選取
open_list
（
index_min_open_list_F
，
：）
=
［］；
%在open_list中將其刪除
close_list
=
［
close_list
；
current_point
］；
%將其加入close_list
map
（
current_point
。
x
，
current_point
。
y
）
=
ValueSet
。
closelist
；
%將新加入的close_list點標記（染色）
DrawMap
（
map
）；
%繪圖
%3。檢查是否符合退出條件
if
current_point
。
x
==
end_point
。
x
&&
current_point
。
y
==
end_point
。
y
b_find_path
=
true
；
break
；
end
%4。 檢查當前點周圍可移動點，並加入open_list中
for
search_dxy
=
SearchDxy
‘
search_point
=
NewSearchPoint
（
current_point
。
x
+
search_dxy
（
1
），
current_point
。
y
+
search_dxy
（
2
））；
key
=
SetKeyStr
（
search_point
。
x
，
search_point
。
y
）；
% 4。1如果它是不可抵達的或者它在 close list 中，忽略它
if
search_point
。
x
<
=
0
||
search_point
。
y
<
=
0
||
map
（
search_point
。
x
，
search_point
。
y
）
==
ValueSet
。
impassable
||
map
（
search_point
。
x
，
search_point
。
y
）
==
ValueSet
。
closelist
continue
；
end
search_point
=
struct2table
（
search_point
）；
search_point
。
G
=
current_point
。
G
+
search_dxy
（
3
）；
%移動代價
search_point
。
H
=
CalcH
（
search_point
。
x
，
search_point
。
y
，
end_point
。
x
，
end_point
。
y
，
HType
）；
%估算成本
search_point
。
F
=
search_point
。
G
+
search_point
。
H
；
search_point
。
parent_key
=
current_point
。
key
；
index_existed_in_openlist
=
find
（
open_list
。
key
==
key
，
1
）；
%判定當前open_list中是否存在該搜尋點
% 4。2如果它不在 open list 中，把它加入 open list ，並且把當前方格設定為它的父親，記錄該方格的 F ， G 和 H 值
if
map
（
search_point
。
x
，
search_point
。
y
）
~=
ValueSet
。
openlist
open_list
=
［
open_list
；
search_point
］；
map
（
search_point
。
x
，
search_point
。
y
）
=
ValueSet
。
openlist
；
%將新加入的open_list點標記（染色）
else
% 4。3如果它已經在 open list 中，檢查這條路徑 （ 即經由當前方格到達它那裡 ） 是否更好，用 G 值作參考。更小的 G 值表示這是更好的路徑。如果是這樣，把它的父親設定為當前方格，並重新計算它的 G 和 F 值。e
if
search_point
。
G
<
open_list
。
G
（
index_existed_in_openlist
）
%若open_list中存在值的G值更大，表示由當前點到達該值更優，將原本儲存點的資訊替換為當前搜尋點資訊
open_list
（
index_existed_in_openlist
，
：）
=
search_point
；
% 進行替換
end
end
end
end
% 如果發現路徑，則按照parent資訊回溯路徑結果
if
b_find_path
%找到路徑
path
=
close_list
（
end
，
：）；
path_step_count
=
1
；
map
（
path
。
x
（
1
），
path
。
y
（
1
））
=
ValueSet
。
path
；
while
path
。
parent_key
（
path_step_count
）
~=
InvalidKeyStr
path_step_count
=
path_step_count
+
1
；
index_parent
=
find
（
close_list
。
key
==
path
。
parent_key
（
path_step_count
-
1
），
1
）；
path
（
path_step_count
，
：）
=
close_list
（
index_parent
，
：）；
map
（
path
。
x
（
path_step_count
），
path
。
y
（
path_step_count
））
=
ValueSet
。
path
；
end
DrawMap
（
map
）；
%繪圖
else
disp
（
’未找到有效路徑！‘
）；
end
disp
（［
’openlist數量： ‘
，
num2str
（
height
（
open_list
）），
’， closelist數量： ‘
，
num2str
（
height
（
close_list
）），
’， 總代價： ‘
，
num2str
（
path
。
G
（
1
），
’%3。2f‘
）］）
%% 估算代價計算
% 參考文章《A*演算法中啟發函式H的選取》
% h_diagonal = 沿著對角線移動的步數， h_straight = 曼哈頓距離，
% 透過考慮所有的對角線移動的步數（每步耗散D2）以及剩下的直線移動的步數（每步耗散D）將這兩者結合在一起。
function
H
=
CalcH
（
x1， y1， x2， y2， type
）
if
nargin
<
5
type
=
3
；
end
dx
=
x2
-
x1
；
dy
=
y2
-
y1
；
switch
type
case
1
H
=
sqrt
（
dx
*
dx
+
dy
*
dy
）；
%歐式距離 乘以係數可以加快搜索距離，但可能造成無解情況
case
2
H
=
abs
（
dx
）
+
abs
（
dy
）；
%曼哈頓距離
case
3
h_diagonal
=
min
（
abs
（
dx
），
abs
（
dy
））；
h_straight
=
abs
（
dx
）
+
abs
（
dy
）；
H
=
sqrt
（
2
）
*
h_diagonal
+
（
h_straight
-
2
*
h_diagonal
）；
%可沿對角移動時的代價函式
case
4
H
=
0
；
%Dijkstra演算法
end
end
%% 繪製map圖
function
DrawMap
（
map
）
global
start_point
end_point
current_point
ValueSet
% 注意這裡對map的操作只是為了顯示效果，不會影響到主函式內的map，
［
map
（
start_point
。
x
，
start_point
。
y
），
map
（
end_point
。
x
，
end_point
。
y
），
map
（
current_point
。
x
，
current_point
。
y
）］
=
deal
（
ValueSet
。
start
，
ValueSet
。
target
，
ValueSet
。
current
）；
imagesc
（
map
’
）
% 注意用plot和imagesc的區別，這裡加了一個轉置
set
（
gca
，
‘XDir’
，
‘normal’
，
‘YDir’
，
‘normal’
）；
%如果不加normal是直接顯示A的結構
pause
（
0。0001
）；
end
%% 搜尋最小代價點
function
index_min_open_list_F
=
SearchOptimalPoint
（
open_list， close_list， current_point， end_point
）
index_min_open_list_F
=
find
（
open_list
。
F
==
min
（
open_list
。
F
））；
%找到最小代價index
if
length
（
index_min_open_list_F
）
>
1
%如果有多個最小代價值，按一定規則優先選取最優解
if
height
（
close_list
）
==
1
%起點時出現，則優先選取同起始/結束連線夾角最接近者
index_min_dyaw_end
=
FindMinDyaw
（
current_point
，
end_point
，
current_point
，
open_list
（
index_min_open_list_F
，
：），
1
）；
index_min_open_list_F
=
index_min_open_list_F
（
index_min_dyaw_end
）；
else
% 否則找到與上一刻方向最接近的點
index_last
=
find
（
close_list
。
key
==
current_point
。
parent_key
，
1
）；
last_point
=
close_list
（
index_last
，
：）；
index_min_dyaw_last
=
FindMinDyaw
（
last_point
，
current_point
，
current_point
，
open_list
（
index_min_open_list_F
，
：））；
index_min_open_list_F
=
index_min_open_list_F
（
index_min_dyaw_last
）；
% 如果還有多個結果，優先選取同當前/結束連線夾角最接近者
if
length
（
index_min_open_list_F
）
>
1
index_min_dyaw_end
=
FindMinDyaw
（
current_point
，
end_point
，
current_point
，
open_list
（
index_min_open_list_F
，
：），
1
）；
index_min_open_list_F
=
index_min_open_list_F
（
index_min_dyaw_end
）；
end
end
end
end
%% 從候選點中選取與目標方向最接近的點
function
index_min_dyaw
=
FindMinDyaw
（
base_point_start， base_point_end， candidate_point_start， candidate_point_end， b_output_single
）
if
nargin
<
5
%是否只輸出唯一值
b_output_single
=
false
；
end
end_yaw
=
atan2
（
base_point_end
。
y
-
base_point_start
。
y
，
base_point_end
。
x
-
base_point_start
。
x
）；
open_list_yaw
=
atan2
（
candidate_point_end
。
y
-
candidate_point_start
。
y
，
candidate_point_end
。
x
-
candidate_point_start
。
x
）；
dyaw
=
abs
（
LimitInPi
（
open_list_yaw
-
end_yaw
））；
if
~
b_output_single
index_min_dyaw
=
find
（
dyaw
==
min
（
dyaw
））；
else
index_min_dyaw
=
find
（
dyaw
==
min
（
dyaw
），
1
，
‘last’
）；
end
end
%% 功能：將輸入角度範圍限制在+-pi以內
function
angle
=
LimitInPi
（
angle
）
% 輸入：弧度
% 輸出：限制在+-pi以內的弧度
angle
=
mod
（
angle
，
2
*
pi
）；
% 對2pi取餘
kk
=
find
（
abs
（
angle
）
>
pi
）；
if
~
isempty
（
kk
）
angle
（
kk
）
=
angle
（
kk
）
-
sign
（
angle
（
kk
））
*
2
*
pi
；
end
end
