原理回顧

路徑規劃演算法學習Day4-Astar演算法

一、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、地圖創建

見路徑規劃演算法學習Day3

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*演算法的完全實作，有問題請評論，歡迎大家查閱指正，