1. 最佳優(yōu)先搜索（Best-First Search）

最佳優(yōu)先搜索（BFS），又稱A算法，是一種啟發(fā)式搜索算法(Heuristic Algorithm)。[不是廣度優(yōu)先搜索算法（ Breadth First Search , BFS )]BFS算法在廣度優(yōu)先搜索的基礎(chǔ)上，用啟發(fā)估價函數(shù)對將要被遍歷到的點進行估價，然后選擇代價小的進行遍歷，直到找到目標節(jié)點或者遍歷完所有點，算法結(jié)束。要實現(xiàn)最佳優(yōu)先搜索我們必須使用一個優(yōu)先隊列（priority queue）來實現(xiàn)，通常采用一個open優(yōu)先隊列和一個closed集，open優(yōu)先隊列用來儲存還沒有遍歷將要遍歷的節(jié)點，而closed集用來儲存已經(jīng)被遍歷過的節(jié)點。1.1 最佳優(yōu)先搜索的過程最佳優(yōu)先搜索的過程可以被描述為：1、將根節(jié)點放入優(yōu)先隊列open中。2、從優(yōu)先隊列中取出優(yōu)先級最高的節(jié)點X。3、根據(jù)節(jié)點X生成子節(jié)點Y:X的子節(jié)點Y不在open隊列或者closed中，由估價函數(shù)計算出估價值，放入open隊列中。X的子節(jié)點Y在open隊列中，且估價值優(yōu)于open隊列中的子節(jié)點Y，將open隊列中的子節(jié)點Y的估價值替換成新的估價值并按優(yōu)先值排序。X的子節(jié)點Y在closed集中，且估價值優(yōu)于closed集中的子節(jié)點Y，將closed集中的子節(jié)點Y移除，并將子節(jié)點Y加入open優(yōu)先隊列。4、將節(jié)點X放入closed集中。5、重復(fù)過程2,3,4直到目標節(jié)點找到，或者open為空，程序結(jié)束。BFS不能保證找到一條最短路徑。然而，它比Dijkstra算法快的多，因為它用了一個啟發(fā)式函數(shù)（heuristic function）快速地導(dǎo)向目標結(jié)點。這篇博客對BFS進行了詳細的介紹用的是羅馬尼亞度假問題https://blog.csdn.net/weixin_46582817/article/details/115217489 ? ?

2. A-Star算法

1968年發(fā)明的A*算法就是把啟發(fā)式方法（heuristic approaches）如BFS，和常規(guī)方如Dijsktra算法結(jié)合在一起的算法。A-Star算法是一種靜態(tài)路網(wǎng)中求解最短路徑最有效的直接搜索方法，也是解決許多搜索問題的有效算法。?和Dijkstra一樣，A*能用于搜索最短路徑。?和BFS一樣，A*能用啟發(fā)式函數(shù)引導(dǎo)它自己。左圖為Astar算法效果圖，右圖為Dijkstra算法效果圖Astar算法與Dijkstra算法的效果差不多，但Astar算法訪問的節(jié)點數(shù)明顯比Dijkstra算法少得多，說明其速度更快，運行時間更短。 2.1 Astar算法所屬分類 A*算法在最短路徑搜索算法中分類為：?直接搜索算法：直接在實際地圖上進行搜索，不經(jīng)過任何預(yù)處理；?啟發(fā)式算法：通過啟發(fā)函數(shù)引導(dǎo)算法的搜索方向；?靜態(tài)圖搜索算法：被搜索的圖的權(quán)值不隨時間變化（后被證明同樣可以適用于動態(tài)圖的搜索 ） 2.2 Astar算法基本概念 A*算法啟發(fā)函數(shù)表示為：f(n)=g(n)+h(n)?f(n) 是從初始狀態(tài)經(jīng)由狀態(tài)n到目標狀態(tài)的代價估計?g(n) 是在狀態(tài)空間中從初始狀態(tài)到狀態(tài)n的實際代價?h(n) 是從狀態(tài)n到目標狀態(tài)的最佳路徑的估計代價（對于路徑搜索問題，狀態(tài)就是圖中的節(jié)點，代價就是距離）Astar算法是啟發(fā)式搜索算法，啟發(fā)式搜索是在狀態(tài)空間中對每一個搜索的位置進行評估，得到最好的位置，再從這個位置進行搜索直到目標。這樣可以省略大量無謂的搜索路徑，提高效率。在啟發(fā)式搜索中，對位置的估價是十分重要的。采用了不同的估價可以有不同的效果。啟發(fā)函數(shù)（Heuristics Function）（估價函數(shù)）：H為啟發(fā)函數(shù)，也被認為是一種試探。由于在找到唯一路徑前，不確定在前面會出現(xiàn)什么障礙物，計算H的算法（例如，H可采用傳統(tǒng)的曼哈頓距離）具體根據(jù)實際場景決定父節(jié)點（parent）：在路徑規(guī)劃中用于回溯的節(jié)點。搜索區(qū)域（The Search Area）：前面圖中的搜索區(qū)域被劃分為了簡單的二維數(shù)組，數(shù)組每個元素對應(yīng)一個小方格，也可以將區(qū)域等分成是五角星，矩形等，通常將一個單位的中心點稱之為搜索區(qū)域節(jié)點（Node）。開放列表(Open List)：將路徑規(guī)劃過程中待檢測的節(jié)點存放于Open List中。關(guān)閉列表(Close List)：將路徑規(guī)劃過程中已經(jīng)檢查過的節(jié)點放在Close List。 2.3 啟發(fā)函數(shù)單調(diào)性的推導(dǎo) 單調(diào)性：單調(diào)性是Astar算法非常重要的一個性質(zhì)，它決定了在用Astar算法進行路徑搜索時，一定能找到一條最優(yōu)路徑。設(shè)父節(jié)點的坐標為(x0,y0)，它的任意一個子節(jié)點的坐標為(xi,yi)，所以對兩者之間的h(x)，就一定滿足 ?在Astar算法的運行過程中，后繼的f(x)值時大于當前f(x)的值，即f(x)在之后對子節(jié)點的搜索擴展是單調(diào)遞增的，不會像人工勢場法一樣出現(xiàn)局部極小值，因此使用Astar算法規(guī)劃出的路徑一定是最優(yōu)路徑.2.4 設(shè)計代價函數(shù)時所需注意的點在使用A*算法的過程中，啟發(fā)代價的值必須盡量接近于真實值（盡量選取能取到的最大值，這樣可以提升搜索效率），以保證規(guī)劃出的路徑盡可能地與實際地圖環(huán)境相符合。根據(jù)所需的模型選擇不同的啟發(fā)代價的計算方法時，同樣必須保證啟發(fā)代價所得的估計值小于真實值。 2.5 代價函數(shù)的選擇 2.5.1 曼哈頓距離 曼哈頓距離函數(shù)在標準坐標系中，表示起始和目標兩節(jié)點的絕對值總和，其估計值就是從當前點做水平和垂直運動，到達目標點的成本的估計，因此，曼哈頓距離函數(shù)中，兩點的距離如下K——相鄰兩節(jié)點之間的距離，即單位距離；
(x1,y1)——起始節(jié)點的坐標;
(x2,y2)——目標節(jié)點的坐標; ?2.5.2 歐幾里得距離 歐幾里得距離又稱為歐式距離，它是衡量兩點之間距離遠近的最常用的方法之一。歐幾里得距離函數(shù)的值可以直接看作起始節(jié)點和目標節(jié)點，在歐式空間中的直線距離，其表達式如下所示K——相鄰兩節(jié)點之間的距離，即單位距離；
(xi,yi)——當前節(jié)點的坐標;
(xk,yk)——目標節(jié)點的坐標;歐幾里德距離函數(shù)作為啟發(fā)代價的計算方法時，雖然尋徑時搜索空間增加從而導(dǎo)致搜索效率的降低，但其所得的估計值最小；而在使用曼哈頓距離函數(shù)時，雖然尋徑需要遍歷的柵格空間少于歐幾里得距離函數(shù)，搜索效率較高，但這種方法得到的估計值與歐幾里得距離函數(shù)相比，距離真實值更遠。 2.6 確定最終路徑 已經(jīng)確定了目標節(jié)點的坐標為，根據(jù)此目標節(jié)點的位置，和先前設(shè)置的方向存儲函數(shù)之中的方向，從目標節(jié)點利用方向反推至起始節(jié)點。具體實現(xiàn)的示意圖如下 ?2.7 路徑平滑 我們需要對規(guī)劃出的路徑進行平滑處理，將路徑的轉(zhuǎn)折處轉(zhuǎn)化為平滑的曲線，使之更加符合無人車的實際運動路徑。主要有基于B樣條曲線的路徑優(yōu)化方法，基于Dubins圓的路徑優(yōu)化方法，和基于Clothoid曲線的路徑優(yōu)化方法，基于貝塞爾曲線的路徑平滑算法。紅色為未平滑路徑，綠色方塊為最終路徑，黃色為貝塞爾曲線擬合得到，藍色為spcrv函數(shù)平滑得到。 2.8 Astar算法的優(yōu)缺點 優(yōu)點：相對于需要將所有節(jié)點展開搜尋的算法，A*算法最大的優(yōu)點就是引入了啟發(fā)信息作為向目標點移動的決策輔助，所以不再需要遍歷整個地圖，降低了計算復(fù)雜度，減少了時間損耗少。缺點：基于柵格法來分割地圖，精度越高，柵格分的就越小，就會使工作量幾何式的增長。估價函數(shù)選取的不合理也有可能導(dǎo)致無法規(guī)劃出最優(yōu)路徑。距離估計與實際值越接近，估價函數(shù)取得就越好。估價函數(shù)f(n)在g(n)一定的情況下，會或多或少的受距離估計值h(n)的制約，節(jié)點距目標點近，h值小，f值相對就小，能保證最短路的搜索向終點的方向進行，明顯優(yōu)于Dijkstra算法的毫無方向的向四周搜索。 2.9 Astar算法流程 如下圖所示，綠色是起點A，紅色是終點B，一系列藍色是障礙物，從A移動到B,繞過障礙。1、用方格（三角形、五角形）劃分空間，簡化搜索區(qū)域。空間被劃分為二維數(shù)組，數(shù)組中每個元素代表空間中的一個方格，可被標記為可行或不可行。未來的路徑就是一系列可行方塊的集合。Nodes的概念涵蓋了一系列可行方塊（或其它方塊）2、將起點A放到Openlist中，Openlist存放著一系列需要檢查的節(jié)點（方塊）3、給Openlist中每個節(jié)點賦值F=G+H （起點為0，橫向和縱向的移動代價為 10 ，對角線的移動代價為 14）4、檢查Openlist，選取F值最小的節(jié)點（此處為A點）。5、將A點從Openlist中刪除，放入Closelist中（Closelist中存放不可尋點，即已被訪問過的點），把A點臨近節(jié)點放入Openlist中，并將A點設(shè)為臨近節(jié)點的父節(jié)點。6、給Openlist中每個節(jié)點賦值，選取F值最小的節(jié)點設(shè)為當前節(jié)點Node，(若當前節(jié)點Node為終點，則尋路結(jié)束，若Openlist中沒有可尋節(jié)點，則尋路失敗）7、檢查當前節(jié)點Node周圍臨近節(jié)點。忽略Closelist中的節(jié)點和不可行節(jié)點（障礙），如果臨近節(jié)點在Openlist中，則對比一下是否從當前節(jié)點到臨近節(jié)點的G值比原G值（直接到臨近節(jié)點的實際代價值）小，若是，把當前節(jié)點作為父節(jié)點。否，不做改動。如果臨近節(jié)點不在Openlist中，將其加入到Openlist中，將當前節(jié)點設(shè)為它的父節(jié)點。8、若上步驟中新節(jié)點未造成任何改動，將新節(jié)點作為當前節(jié)點，重復(fù)6,7）中的步驟，直到找到目標節(jié)點。尋找到目標節(jié)點9、從目標節(jié)點回溯可以找到初始點，從而確定路徑上述描述路徑的圖片有些錯誤，具體步驟如下圖所示。 ? ?

3. 其他Astar算法

3.1 Astar——三維地圖規(guī)劃 3.1.1 3D-Astar原理三維柵格地圖，顧名思義不是簡單的二維平面，它必須得有三維方向，也就是高度方向上的拓展。柵格地圖在XY水平面上的柵格的投影顏色不盡相同，柵格黃色程度越高，表明此處的高度越高。為了使啟發(fā)代價的值應(yīng)該盡量接近于真實值，三維柵格地圖中仍然選用歐幾里得距離作為估價函數(shù)計算啟發(fā)代價的計算方法。但本次實驗所處的環(huán)境為三維避障空間，因此相較于二維空間的路徑規(guī)劃，我們將公式做三維化推廣，具體形式如下：h(n)——當前節(jié)點到目標節(jié)點的啟發(fā)代價值；g(n-1)——當前節(jié)點到它的父節(jié)點之間的路徑代價；D(n-1, n)——當前節(jié)點與它的子節(jié)點之間的代價值。g(n-1)與二維規(guī)劃中的距離代價計算方法一致，但D(n-1, n)在計算時用父節(jié)點與子節(jié)點之間的距離值除以三維避障環(huán)境中設(shè)定的柵格可通行程度，h(n)也是用當前節(jié)點到目標節(jié)點的啟發(fā)代價值除以地圖環(huán)境中預(yù)先設(shè)定的柵格可通行程度。 3.1.2 基于MATLAB實現(xiàn)3D-Astar 這里是代碼的GitHub地址：https://github.com/ybmasmiling/Astar_3D ?3.2 距離與能量復(fù)合Astar算法 經(jīng)典Astar算法路徑規(guī)劃是取兩節(jié)點間曼哈頓距離作為距離估計為最優(yōu)原則搜索路徑，從而得到最短路徑。搜索路徑的過程中并沒有考慮實際道路坡度和道路滾動阻力系數(shù)對行駛車輛最短路徑搜索的影響，也沒有考慮在道路上行駛的能量損耗問題，即經(jīng)典Astar算法搜索的最短路徑并非符合實際車輛行駛的最優(yōu)路徑。 3.2.1 最短路徑啟發(fā)函數(shù)最短路徑啟發(fā)函數(shù): ?最終得到的最短路徑啟發(fā)函數(shù)如下式： ?3.2.2 最短道路損耗功啟發(fā)函數(shù) 道路損耗功啟發(fā)函數(shù): ?最終得到的最短道路損耗功啟發(fā)函數(shù)如下式： ?3.2.3 綜合啟發(fā)函數(shù) 綜合啟發(fā)函數(shù)：綜合式啟發(fā)函數(shù)統(tǒng)一最短路徑啟發(fā)函數(shù)和最小道路額外功函數(shù)，不同的權(quán)重大小決定最短路徑啟發(fā)函數(shù)和最小道路額外功函數(shù)在綜合式啟發(fā)函數(shù)中所占不同的比重。 3.3 雙向Astar算法 傳統(tǒng)A算法在大環(huán)境中搜索，存在著搜索效率低的問題。傳統(tǒng)A算法是從起點開始向終點搜索，直到尋到可行路徑停止搜索，在搜索路徑的前期階段遠g(n)小于h(n),搜索時橫向搜索范圍窄，縱向搜索范圍寬，搜索速度快，在搜索的后期階段g(n)遠大于h(n),搜索時縱向搜索范圍窄，橫向搜索范圍寬，搜索速度慢；**而改進后的雙向A搜索算法分別從起點和終點開始搜索，當搜索到相同的當前節(jié)點時，搜索結(jié)束。**相比于傳統(tǒng)A算法，雙向A*搜索算法都處于g(n)遠小于h(n)階段，一定程度上提高了算法的搜索效率，縮短搜索時間。 ? ?

4. MATLAB實現(xiàn)Astar算法

4.1 defColorMap.m 用于初始化地圖參數(shù)

function [field,cmap] = defColorMap(rows, cols)
cmap = [1 1 1; ...    % 1-白色-空地
  0 0 0; ...      % 2-黑色-靜態(tài)障礙
  1 0 0; ...      % 3-紅色-動態(tài)障礙
  1 1 0;...      % 4-黃色-起始點 
  1 0 1;...      % 5-品紅-目標點
  0 1 0; ...      % 6-綠色-到目標點的規(guī)劃路徑  
  0 1 1];       % 7-青色-動態(tài)規(guī)劃的路徑


% 構(gòu)建顏色MAP圖
colormap(cmap);


% 定義柵格地圖全域，并初始化空白區(qū)域
field = ones(rows, cols);


% 障礙物區(qū)域
obsRate = 0.3;
obsNum = floor(rows*cols*obsRate);
obsIndex = randi([1,rows*cols],obsNum,1);
field(obsIndex) = 2;

4.2 getChildNode.m 用于獲取子節(jié)點信息

function childNodes = getChildNode(field,closeList, parentNode)
% 選取父節(jié)點周邊8個節(jié)點作為備選子節(jié)點，線性化坐標
% 排除超過邊界之外的、位于障礙區(qū)的、位于closeList中的


[rows, cols] = size(field);
[row_parentNode, col_parentNode] = ind2sub([rows, cols], parentNode);
childNodes = [];
closeList = closeList(:,1);


% 第1個子節(jié)點(右節(jié)點)
childNode = [row_parentNode, col_parentNode+1];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
    childNode(2) < 1 || childNode(2) > cols)
  if field(childNode(1), childNode(2)) ~= 2
    childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2));
    if ~ismember(childNode_LineIdx, closeList)
      childNodes(end+1) = childNode_LineIdx;
    end
  end
end


% 第2個子節(jié)點（右上節(jié)點）
childNode = [row_parentNode-1, col_parentNode+1];
child_brother_node_sub1 = [row_parentNode-1, col_parentNode];
child_brother_node_sub2 = [row_parentNode, col_parentNode+1];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
    childNode(2) < 1 || childNode(2) > cols)
  if field(childNode(1), childNode(2)) ~= 2
    if ~(field(child_brother_node_sub1(1), child_brother_node_sub1(2)) == 2 & field(child_brother_node_sub2(1), child_brother_node_sub2(2)) == 2)
      childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2));
      if ~ismember(childNode_LineIdx, closeList)
        childNodes(end+1) = childNode_LineIdx;
      end
    end  
  end
end




% 第3個子節(jié)點（上節(jié)點）
childNode = [row_parentNode-1, col_parentNode];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
    childNode(2) < 1 || childNode(2) > cols)
  if field(childNode(1), childNode(2)) ~= 2
    childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2));
    if ~ismember(childNode_LineIdx, closeList)
      childNodes(end+1) = childNode_LineIdx;
    end
  end
end




% 第4個子節(jié)點（左上）
childNode = [row_parentNode-1, col_parentNode-1];
child_brother_node_sub1 = [row_parentNode-1, col_parentNode];
child_brother_node_sub2 = [row_parentNode, col_parentNode-1];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
    childNode(2) < 1 || childNode(2) > cols)
  if field(childNode(1), childNode(2)) ~= 2
    if ~(field(child_brother_node_sub1(1), child_brother_node_sub1(2)) == 2 & field(child_brother_node_sub2(1), child_brother_node_sub2(2)) == 2)
      childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2));
      if ~ismember(childNode_LineIdx, closeList)
        childNodes(end+1) = childNode_LineIdx;
      end
    end 
  end
end




% 第5個子節(jié)點(左節(jié)點)
childNode = [row_parentNode, col_parentNode-1];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
    childNode(2) < 1 || childNode(2) > cols)
  if field(childNode(1), childNode(2)) ~= 2
    childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2));
    if ~ismember(childNode_LineIdx, closeList)
      childNodes(end+1) = childNode_LineIdx;
    end
  end
end




% 第6個子節(jié)點（左下）
childNode = [row_parentNode+1, col_parentNode-1];
  child_brother_node_sub1 = [row_parentNode, col_parentNode-1];
  child_brother_node_sub2 = [row_parentNode+1, col_parentNode];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
    childNode(2) < 1 || childNode(2) > cols)
  if field(childNode(1), childNode(2)) ~= 2
    if ~(field(child_brother_node_sub1(1), child_brother_node_sub1(2)) == 2 & field(child_brother_node_sub2(1), child_brother_node_sub2(2)) == 2)
      childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2));
      if ~ismember(childNode_LineIdx, closeList)
        childNodes(end+1) = childNode_LineIdx;
      end
    end 
  end
end




% 第7個子節(jié)點（下）
childNode = [row_parentNode+1, col_parentNode];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
    childNode(2) < 1 || childNode(2) > cols)
  if field(childNode(1), childNode(2)) ~= 2
    childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2));
    if ~ismember(childNode_LineIdx, closeList)
      childNodes(end+1) = childNode_LineIdx;
    end
  end
end




% 第8個子節(jié)點（右下）
childNode = [row_parentNode+1, col_parentNode+1];
  child_brother_node_sub1 = [row_parentNode, col_parentNode+1];
  child_brother_node_sub2 = [row_parentNode+1, col_parentNode];
if ~(childNode(1) < 1 || childNode(1) > rows ||...
    childNode(2) < 1 || childNode(2) > cols)
  if field(childNode(1), childNode(2)) ~= 2 
    if ~(field(child_brother_node_sub1(1), child_brother_node_sub1(2)) == 2 & field(child_brother_node_sub2(1), child_brother_node_sub2(2)) == 2)
      childNode_LineIdx = sub2ind([rows, cols], childNode(1), childNode(2)); 
      if ~ismember(childNode_LineIdx, closeList)
        childNodes(end+1) = childNode_LineIdx;
      end
    end
  end
end

4.3 Astar.m 主程序

% 基于柵格地圖的機器人路徑規(guī)劃算法
% A*算法
clc
clear
close all


%% 柵格界面、場景定義
% 行數(shù)和列數(shù)
rows = 20;
cols = 20;
[field,cmap] = defColorMap(rows, cols);


% 起點、終點、障礙物區(qū)域
startPos = 2;
goalPos = rows*cols-2;
field(startPos) = 4;
field(goalPos) = 5;


%% 預(yù)處理


% 初始化closeList
parentNode = startPos;
closeList = [startPos,0];


% 初始化openList
openList = struct;
childNodes = getChildNode(field,closeList,parentNode);
for i = 1:length(childNodes)
  [row_startPos,col_startPos] = ind2sub([rows, cols],startPos);
  [row_goalPos,col_goalPos] = ind2sub([rows, cols],goalPos);  
  [row,col] = ind2sub([rows, cols],childNodes(i));


  % 存入openList結(jié)構(gòu)體
  openList(i).node = childNodes(i);
  openList(i).g = norm([row_startPos,col_startPos] - [row,col]); % 實際代價采用歐式距離
  openList(i).h = abs(row_goalPos - row) + abs(col_goalPos - col); % 估計代價采用曼哈頓距離
  openList(i).f = openList(i).g + openList(i).h;
end


% 初始化path
for i = 1:rows*cols
  path{i,1} = i; % 線性索引值
end
for i = 1:length(openList)
  node = openList(i).node;
  path{node,2} = [startPos,node];
end 


%% 開始搜索
% 從openList開始搜索移動代價最小的節(jié)點
[~, idx_min] = min([openList.f]); 
parentNode = openList(idx_min).node;


% 進入循環(huán)
while true 
  
  % 找出父節(jié)點的8個子節(jié)點，障礙物節(jié)點用inf，
  childNodes = getChildNode(field, closeList,parentNode);
  
  % 判斷這些子節(jié)點是否在openList中，若在，則比較更新；沒在則追加到openList中
  for i = 1:length(childNodes)
    
    % 需要判斷的子節(jié)點
    childNode = childNodes(i);
    [in_flag,idx] = ismember(childNode, [openList.node]);
      
    % 計算代價函數(shù)
    [row_parentNode,col_parentNode] = ind2sub([rows, cols], parentNode);
    [row_childNode,col_childNode] = ind2sub([rows, cols], childNode);
    [row_goalPos,col_goalPos] = ind2sub([rows, cols],goalPos);
    g = openList(idx_min).g + norm( [row_parentNode,col_parentNode] -...
      [row_childNode,col_childNode]);
    h = abs(row_goalPos - row_childNode) + abs(col_goalPos - col_childNode); % 采用曼哈頓距離進行代價估計
    f = g + h;
    
    if in_flag  % 若在，比較更新g和f    
      if f < openList(idx).f
        openList(idx).g = g;
        openList(idx).h = h;
        openList(idx).f = f;
        path{childNode,2} = [path{parentNode,2}, childNode];
      end
    else     % 若不在，追加到openList
      openList(end+1).node = childNode;
      openList(end).g = g;
      openList(end).h = h;
      openList(end).f = f;
      path{childNode,2} = [path{parentNode,2}, childNode];
    end
  end
    
  % 從openList移出移動代價最小的節(jié)點到closeList
  closeList(end+1,: ) = [openList(idx_min).node, openList(idx_min).f];
  openList(idx_min)= [];


  % 重新搜索：從openList搜索移動代價最小的節(jié)點
  [~, idx_min] = min([openList.f]);
  parentNode = openList(idx_min).node;
  
  % 判斷是否搜索到終點
  if parentNode == goalPos
    closeList(end+1,: ) = [openList(idx_min).node, openList(idx_min).f];
    break
  end
end


%% 畫路徑
% 找出目標最優(yōu)路徑
path_target = path{goalPos,2};
for i = 1:rows*cols
   if ~isempty(path{i,2}) 
    field((path{i,1})) = 7;
   end
end
field(startPos) = 4;
field(goalPos) = 5;
field(path_target(2:end-1)) = 6;


% 畫柵格圖
image(1.5,1.5,field);
grid on;
set(gca,'gridline','-','gridcolor','k','linewidth',2,'GridAlpha',0.5);
set(gca,'xtick',1:cols+1,'ytick',1:rows+1);
axis image;
hold on;
[y0,x0] = ind2sub([rows,cols], path_target);
y = y0 + 0.5;
x = x0 + 0.5;
plot(x,y,'-','Color','r','LineWidth',2.5);
hold on;
points = [x',y'];
M = 10;
[x1,y1] = bezir_n(points, M);
plot(x1,y1,'-','Color','y','LineWidth',2.5);
hold on;
values = spcrv([[x(1) x x(end)];[y(1) y y(end)]],3);
plot(values(1,:),values(2,:),'b','LineWidth',2.5);

4.4 算法效果 運行總時間柵格地圖大小（20x20）柵格地圖大小（30x30）柵格地圖大小（40x40）可以看到Astar算法對于柵格地圖越大的情況，搜索時間越長，其速度相比Dijkstra有優(yōu)勢（尤其是在地圖比較大的時候，優(yōu)勢更明顯）。但其總耗時較長，不適用于實時的路徑規(guī)劃，不適用于局部路徑規(guī)劃，適用于全局路徑規(guī)劃。

5. python實現(xiàn)Astar算法

可以參考這篇文章https://www.jianshu.com/p/5704e67f40aa這篇文章介紹了Astar以及后續(xù)的變種算法python 版本的偽代碼（來源：https://brilliant.org/wiki/a-star-search/）如下：

make an openlist containing only the starting node
make an empty closed list
  while (the destination node has not been reached):
    consider the node with the lowest f score in the open list
    if (this node is our destination node) :
      we are finished 
    if not:
      put the current node in the closed list and look at all of its neighbors
      for (each neighbor of the current node):
        if (neighbor has lower g value than current and is in the closed list) :
          replace the neighbor with the new, lower, g value 
          current node is now the neighbor's parent      
        else if (current g value is lower and this neighbor is in the open list ) :
          replace the neighbor with the new, lower, g value 
          change the neighbor's parent to our current node


        else if this neighbor is not in both lists:
          add it to the open list and set its g

6. Java實現(xiàn)Astar算法

可以參考這篇文章：https://www.jianshu.com/p/950233af52df

7. 實踐案例—基于ROS實現(xiàn)Astar與DWA算法

本項目以Astar算法作為全局路徑規(guī)劃算法，DWA作為局部路徑規(guī)劃算法，實現(xiàn)效果如下。（具體原理與算法代碼解釋與說明會在之后的文章附上）