a*演算法是啟發式搜尋，是一種盡可能基於現有資訊的搜尋策略，也就是說搜尋過程中盡量利用目前已知的諸如迭代步數，以及從初始狀態和當前狀態到目標狀態估計所需的費用等資訊。
a*演算法可以選擇下乙個被檢查的節點時引入了已知的全域性資訊，對當前結點距離終點的距離作出估計，作為評價該節點處於最優路線上的可能性的量度，這樣可以首先搜尋可能性大的節點，從而提高了搜尋過程的效率。
a*演算法的基本思想如下：引入當前節點j的估計函式f*,當前節點j的估計函式定義為：
f*(j)= g(j)+h*(j)
其中g(j)是從起點到當前節點j的實際費用的量度，h*(j)是從節點j到終點的最小費用的估計，可以依據實際情況，選擇h*(j)的具體形式，h*(j)要滿足乙個要求：不能高於節點j到終點的實際最小費用。從起始節點點向目的節點搜尋時，每次都搜尋f*(j)最小的節點，直到發現目的節點。
a*演算法的核心是設計估價函式，設計估價函式h(j)有很多方法，下面介紹其中的兩種。
估價函式1：歐幾里德距離
可以證明，在和起點距離相等的中間節點集合裡，與終點直線距離（歐幾里德距離）越小的節點，方向夾角越小。假設在圖2-3求s到g點的最短路徑，與s最近的點是節點2，與g最近的是節點11，因此可以確定上路點是節點2，下路點是節點11，求s到g點的最短路徑就是求節點2到節點11的路徑。定義l(i,j)表示從節點i到節點j的有向線段，angle[ l(i,j),l(a,b) ]表示有向線段ij和有向線段ab的夾角。由於angle[ l(2,11),l(2,3) ]< angle[l(2,11),l(2,1) ] ,因此選擇節點3，而與節點3連線的節點有節點4和節點7，angle[ l(3,11),l(3,7) ]< angle[ l(3,11),l(3,4) ]，選擇節點7，與節點7相連的節點有節點8，節點11，和節點6，由於angle[l(7,11),l(7,11) ]< < angle[ l(7,11),l(7,6) ]假設起點s的座標（sx,sy），終點g的座標(gx,gy),中間點n的座標(nx,ny),
估價函式取歐幾里德距離，表示為：
這個估價函式的計算量很大，不利於海量資料的路徑規劃計算。
估價函式2：曼哈頓距離
利用歐幾里德距離計算估價函式的計算量很大，因此考慮將兩點之間的曼哈頓距離作為估價函式。其中a點的經緯度為(ax,ay),b點的經緯度是(bx,by),則a、b之間的manhattan距離可以表示為:
m(a,b)=p*(|ax-bx|+|ay-by|)
p = 2*∏*r / 360
由於p是常數，可以簡化為
f(j)=g(j)+(|ax-bx|+|ay-by|)
此估價函式計算量小，雖然不是嚴格的方向優先，但基本能保證最短路徑的搜尋方向向目標點的方向進行。
a*演算法在搜尋中設定兩個表:open表和close表。open表儲存了所有已生成而未被考察的節點，close表中記錄已被考察過的節點。演算法中有一步是根據估計函式重排open表中的節點，這樣，迴圈的每一步只考慮open表中狀態最好（代價最小）的節點，如果被發現在open表中存在同乙個節點，就應比較兩個節點代價的大小，如擴充套件得到的新節點代價大於已有的節點代價，可以放棄擴充套件得到的新節點，否則就以新節點代替原來的節點。如果在close表中存在同乙個節點號，則新新生成的路徑的代價肯定大於原來路徑的代價，可以淘汰新生成的節點。
演算法步驟如下：
步1：生成空的open表、close表，將起點s放入open表
步2：如果open表為空，則失敗退出。
步3：從open表中，找出頭節點u,作為當前節點,將它從open表中移除。
步4：判斷u是否為終點，如果是，則通過節點u的父指標，一直遍歷到起點，找到到最優路徑，演算法結束。否則，轉步5.
步5：擴充套件當前節點u,找到其擴充套件的後繼節點集合v,遍歷集合v中的節點，如果節點即不在open表也不在close表，計算該節點的估計值，將節點加入到open表，設定父節點為u.如果節點在open表、close表，比較當前節點的代價g(v)和open表、close表中代價，如果當前節點的代價g(v)小，更新表中的代價和父節點指標。
步6：按估價值遞增的順序，對open表中所有節點進行排序。
步7：轉步2.
舉乙個例子來說。有路網如下圖，假設需要從s節移動到g節點，則a*演算法的計算過程如下：
1）初始狀態：設定節點0的f(x)=0, 節點0進open表，尋路成功標誌bfind為false;設定close表為空。
2） 估算節點0，考察與節點0相連的所有節點（只有節點1）是否是目標節點，如果不是則估算這些節點的f(x)值，將其放進open表,同時節點0被放入close表；最後需要將open表中的節點按f(x)的值從小到大的順序排列。
3）從open表取出f(x)值最小的節點（節點1），考察與節點1相連的所有節點（節點2，6）是否是目標節點，如果不是則估算這些節點的f(x)值，將其放進open表，同時節點1被放入close表。最後需要將open表中的節點按f(x)的值從小到大的順序排列（由於節點2比節點6接近目標節點，因此節點2的f(x)值小於節點6的f(x)值，節點2排在節點6的前面）。
4） 從open表取出f(x)值最小的節點（節點2），考察與節點2相連的所有節點（節點3）是否是目標節點，如果不是則估算這些節點的f(x)值，將其放進open表，同時節點2被放入close表。將open表中的節點按f(x)的值從小到大的順序排列。
5）從open表取出f(x)值最小的節點（節點3），考察與節點3相連的所有節點（節點4）是否是目標節點，如果不是則估算這些節點的f(x)值，將其放進open表，同時節點3被放入close表。將open表中的節點按f(x)的值從小到大的順序排列。
6）從open表取出f(x)值最小的節點（節點4），考察與節點4相連的所有節點（節點5）是否是目標節點，如果是則將節點4被放入close表，然後將考察的節點（節點5）放入close表，此時發現一條叢開始地到目的地的路徑。
7） 從節點5開始往前回溯,從close表中提取路徑。
根據以上思路，a*演算法的偽**如下：
機械人自主導航路徑規劃實現演算法介紹
機械人實現自主導航就是實現機械人自主完成行駛任務，自主避障。功能需要 定位 建圖 規劃 定位 確定機械人位置資訊 建圖 識別建立環境地圖，以柵格占用地圖為主。地圖分為 占用柵格地圖及代價地圖 costmap 其中占用柵格地圖表述地圖的靜態障礙物資訊 代價地圖 costmap 用於導航，機械人以最小c...
全域性路徑規劃之 Dijkstra演算法
通過dijkstra計算圖g中的最短路徑時，需要指定起點s 即從頂點s開始計算 此外，引進兩個集合s和u。s的作用是記錄已求出最短路徑的頂點 以及相應的最短路徑長度 而u則是記錄還未求出最短路徑的頂點 以及該頂點到起點s的距離 初始時，s中只有起點s u中是除s之外的頂點，並且u中頂點的路徑是 起點...
導航與路徑跟蹤演算法
入門參考 路徑規劃演算法 1.a 演算法 a 演算法的特點是設計出評價函式。可參考的入門部落格有導航路徑規劃之五 a 演算法 2.prm路徑規劃演算法是一種基於圖搜尋的方法，它將連續空間轉換成離散空間，再利用a 等搜尋演算法在路線圖上尋找路徑，以提高搜尋效率。3.d 演算法是動態a d star,d...