路徑規劃算法學習Day5-A*算法的實現
原理回顧
一、A* 算法評價函數
評價函數:f(n)=h(n)+g(n);
A* 可以看作Dijkstar和貪婪算法的結合，Dijkstar在規劃中注重的是當前節點到起點的總代價值。而貪婪算法注重的是當前節點到終點的總代價值。
所以將評價函數定義爲f(n),h(n)則爲貪婪算法的權重代價，g(n)爲Dijkstar的權重代價。
根據評價函數f(n)，A* 函數就會有目的性的去規劃路徑。
二、曼哈頓距離（Manhattan Distance）
出租車幾何或曼哈頓距離（Manhattan Distance）是由十九世紀的赫爾曼·閔可夫斯基所創詞彙 ，是種使用在幾何度量空間的幾何學用語，用以標明兩個點在標準座標系上的絕對軸距總和。
2.1、名詞解釋
圖1中紅線代表曼哈頓距離，綠色代表歐氏距離，也就是直線距離，而藍色和黃色代表等價的曼哈頓距離。曼哈頓距離——兩點在南北方向上的距離加上在東西方向上的距離，即d(i,j)=|xi-xj|+|yi-yj|。對於一個具有正南正北、正東正西方向規則佈局的城鎮街道，從一點到達另一點的距離正是在南北方向上旅行的距離加上在東西方向上旅行的距離，因此，曼哈頓距離又稱爲出租車距離。曼哈頓距離不是距離不變量，當座標軸變動時，點間的距離就會不同。曼哈頓距離示意圖在早期的計算機圖形學中，屏幕是由像素構成，是整數，點的座標也一般是整數，原因是浮點運算很昂貴，很慢而且有誤差，如果直接使用AB的歐氏距離(歐幾里德距離：在二維和三維空間中的歐氏距離的就是兩點之間的距離），則必須要進行浮點運算，如果使用AC和CB，則只要計算加減法即可，這就大大提高了運算速度，而且不管累計運算多少次，都不會有誤差。
我們可以定義曼哈頓距離的正式意義爲L1-距離或城市區塊距離，也就是在歐幾里德空間的固定直角座標系上兩點所形成的線段對軸產生的投影的距離總和。
例如在平面上，座標(x1,y1)的i點與座標(x2,y2)的j點的曼哈頓距離爲：
d(i,j)=|X1-X2|+|Y1-Y2|.
此原理參考：曼哈頓距離
2.2、其它啓發函數
當然，啓發函數不單單曼哈頓距離一種，常見的還有歐式距離、切比雪夫距離等等。
歐式距離
切比雪夫距離
二、A* 算法matlab完全實現
3.1、地圖創建
3.2、A*算法matlab主程序
load('Map50.mat');
start_node = [1, 1];
dest_node = [45, 46];
figure(1);
[path, iterations] = Astar1(map, start_node, dest_node);
3.3、A*算法matlab主程序
function [path,iterations]=Astar1(Map,origin,destination)
Heuristic_ratio = 1;%啓發式函數比例
iterations=0;%記錄迭代次數
%圖像顏色給定
white = [1,1,1];%1
black = [0,0,0];%2
green = [0,1,0];%3
yellow = [1,1,0];%4
red = [1,0,0];%5
blue = [0,0,1];%6
cyan = [0,1,1];%7
color_list = [white; black; green; yellow; red; blue; cyan];
colormap(color_list);
Obstacle = 2;%障礙
Origin = 3;%開始
Destination = 4;%終點
Finished = 5;%完成搜索
Unfinished = 6;%未完成搜索
Path = 7;%路徑
[rows, cols]=size(Map);
%創建地圖
logical_map = logical(Map);
map = zeros(rows,cols);
map(logical_map) = 2;%Barrier
map(~logical_map) = 1;%free
%list----g
node_g_list = Inf(rows, cols);
node_g_list(origin(1), origin(2)) = 0;
%list----f
node_f_list = Inf(rows, cols);
node_f_list(origin(1), origin(2)) = Heuristic(origin, destination, Heuristic_ratio);
% 創建父節點列表
parent_list = zeros(rows,cols);
destination_index = sub2ind(size(Map), destination(1), destination(2));
origin_index = sub2ind(size(Map), origin(1), origin(2));
Open_list = [origin_index];
plan_succeeded = false;
%迭代循環
while 1
iterations = iterations+1;
map(origin(1), origin(2)) = Origin;
map(destination(1), destination(2)) = Destination;
[min_node_cost, current_node_index] = min(node_f_list(:));
if(min_node_cost == inf || current_node_index == destination_index)
plan_succeeded = true;
break;
end
node_f_list(current_node_index) = inf;
map(current_node_index) = Finished;
[i,j] = ind2sub(size(map), current_node_index);
for k = 0:3 % four direction
if(k == 0)
adjacent_node = [i-1,j];
elseif (k == 1)
adjacent_node = [i+1,j];
elseif (k == 2)
adjacent_node = [i,j-1];
elseif(k == 3)
adjacent_node = [i,j+1];
end
if((adjacent_node(1) > 0 && adjacent_node(1) <= rows) && (adjacent_node(2) > 0 && adjacent_node(2) <= cols)) %保證搜索不超過地圖界限
if(map(adjacent_node(1),adjacent_node(2)) ~= Obstacle && map(adjacent_node(1),adjacent_node(2)) ~= Finished)
if(node_g_list(adjacent_node(1),adjacent_node(2)) > min_node_cost + 1 )
node_g_list(adjacent_node(1),adjacent_node(2)) = node_g_list(current_node_index) + 1;
node_f_list(adjacent_node(1),adjacent_node(2)) = node_g_list(adjacent_node(1),adjacent_node(2)) + Heuristic(adjacent_node, destination, Heuristic_ratio);
if(map(adjacent_node(1),adjacent_node(2)) == Origin)
parent_list(adjacent_node(1),adjacent_node(2)) = 0; %如果鄰接節點是原點，則設置父節點爲0。.
else
parent_list(adjacent_node(1),adjacent_node(2)) = current_node_index;
%設置父當前節點索引
end
if(map(adjacent_node(1),adjacent_node(2)) ~= Unfinished)
map(adjacent_node(1),adjacent_node(2)) = Unfinished;
%將相鄰節點標記爲未完成
end
end
end
end
end
end
if(plan_succeeded)
path = [];
node = destination_index;
while(parent_list(node) ~= 0)
path = [parent_list(node), path];
node = parent_list(node);
end
for k = 2:size(path,2)
map(path(k)) = 7;
image(0.5,0.5,map);
grid on;
set(gca,'xtick',1:1:rows);
set(gca,'ytick',1:1:rows);
axis image;
drawnow;
end
else
path = [];
end
end
四、A* 算法50*50地圖演示
五、總結
此文爲A*算法的完全實現，有問題請評論，歡迎大家查閱指正。