引子
我們討論一個移動機器人遇到問題：如何移動到指定位置
- 首先，移動機器人需要有一個地圖，同時知道自己現在在哪兒，同時要知道指定位置在地圖的坐標，中途哪兒有障礙物。這個問題就是位置環境中的自主定位與建圖，也就是SLAM。
- 其次，我們假設目前是在A點，目標位置是B點。需要求得從A點到B點最短的路徑。這個問題就是路徑規劃。
- 再次，已知地圖中最短的路徑，我們需要用路徑求得實際的運動參數，這里稱之為軌跡。這個問題就是路徑解析。
- 最后，有了軌跡，根據軌跡的運動參數驅動移動機器人實際運行。這個問題就是運動控制實現。
這里討論的是路徑規劃問題。而A*算法是廣為使用的解決這個問題的算法。
基本概念
- 啟發式搜索：啟發式搜索就是在狀態空間中的搜索對每一個搜索的位置進行評估，得到最好的位置，再從這個位置進行搜索直到目標。這樣可以省略大量無畏的搜索路徑，提到了效率。在啟發式搜索中，對位置的估價是十分重要的。采用了不同的估價可以有不同的效果。
- 估價函數：從當前節點移動到目標節點的預估費用；這個估計就是啟發式的。在尋路問題和迷宮問題中，我們通常用曼哈頓（manhattan）估價函數（下文有介紹）預估費用。
- start：路徑規劃的起始點，也是機器人當前位置或初始位置A
- goal：路徑規劃的終點，也是機器人想要到達的位置B
- g_score：當前點到沿著start點A產生的路徑到A點的移動耗費
- h_score：不考慮不可通過區域，當前點到goal點B的理論移動耗費
- f_score：g_score+h_score，通常也寫為F=G+H
- 開啟列表openset：尋路過程中的待檢索節點列表
- 關閉列表closeset：不需要再次檢索的節點列表
- 追溯表comaFrom：存儲父子節點關系的列表，用于追溯生成路徑。
算法解析
如圖1，綠色點為start設為A，紅色點為goal設為B，藍色點為不可通過的障礙物，黑色點為自由區域。目標是規劃從A到B的路徑。
圖1
開始搜索
- 搜索的從A點開始，首先將A點加入開啟列表，此時取開啟列表中的最小值，初始階段開啟列表中只有A一個節點，因此將A點從開啟列表中取出，將A點加入關閉列表。
-
取出A點的相鄰點，將相鄰點加入開啟列表。如圖2所示，此時A點即為相鄰點的父節點。圖中箭頭指向父節點。將相鄰點與A點加入追溯表中。
圖2
計算耗費評分
對相鄰點，一次計算每一點的g_score，h_score，最后得到f_score。如圖3，節點的右下角為g_score值，左下角為h_score值，右上角為f_score。
圖3
選最小值，再次搜索
- 選出開啟列表中的F值最小的節點，將此節點設為當前節點，移出開啟列表，同時加入關閉列表。如圖4所示。
-
取出當前點的相鄰點，當相鄰點為關閉點或者墻時，不操作。此外，查看相鄰點是否在開啟列表中，如不在開啟列表中將相鄰點加入開啟列表。如相鄰點已經在開啟列表中，則需要進行G值判定
圖4
G值判定
- 對于相鄰點在開啟列表中的，計算相鄰點的G值，計算按照當前路徑的G值與原開啟列表中的G值大小。如果當前路徑G值小于原開啟列表G值，則相鄰點以當前點為父節點，將相鄰點與當前點加入追溯表中。同時更新此相鄰點的H值。如果當前路徑G值大于等于原開啟列表G值，則相鄰點按照原開啟列表中的節點關系，H值不變。因為圖示中，當前點G值比原開啟列表G值大，因此節點關系按照原父子關系和F值。
計算耗費評分，選最小值
-
此時計算開啟列表中F值最小的點，將此節點設為當前節點，并列最小F值的按添加開啟列表順序，以最新添加為佳。
圖5
重復搜索判定工作
- 直到當goal點B加入開啟列表中，則搜索完成。此時事實上生成的路徑并一定是最佳路徑，而是最快計算出的路徑。若判定標準改為當goal點B加入關閉列表中搜索完成，則得出路徑是最佳路徑，但此時計算量較前者大。
-
當沒有找到goal點，同時開啟列表已空，則搜索不到路徑。結束搜索。
圖6
生成路徑
-
由goal點B向上逐級追溯父節點，追溯至起點A，此時各節點組成的路徑即使A*算法生成的最優路徑。
圖7
算法實現
- 初始化參數
- 起始點start
- 終點goal
- h_score
- g_socre
- f_score
- 開啟列表openset
- 關閉列表closeset
- 追溯表comeFrom
- 程序主體
- 將起始點start加入開啟列表openset
- 重復一下工作
- 尋找開啟列表openset中F值最小的節點，設為當前點current
- 開啟列表openset中移出當前點current
- 關閉列表openset中加入當前點current
- 對當前點的每一個相鄰點neighbor
- 如果它不可通過或者已經在關閉列表中，略過。否則：
- 如果它不在開啟列表中，加入開啟列表中
- 如果在開啟列表中，G值判定，若此路徑G值比之前路徑小，則此相鄰點的父節點為當前點，同時更新G與F值。反之，則保持原來的節點關系與G、F值。
- 當目標點goal在開啟列表中，則結束程序，此時有路徑生成，此時由goal節點開始逐級追溯上一級父節點，直到追溯到開始節點start，此時各節點即為路徑。
- 當開啟列表為空，則結束程序，此時沒有路徑
偽代碼
// a* 偽代碼
function A*(start, goal)
//初始化關閉列表，已判定過的節點，進關閉列表。
closedSet := {}
// 初始化開始列表，待判定的節點加入開始列表。
// 初始openset中僅包括start點。
openSet := {start}
// 對每一個節點都只有唯一的一個父節點，用cameFrom集合保存節點的子父關系。
//cameFrom（節點）得到父節點。
cameFrom := the empty map
// gScore估值集合
gScore := map with default value of Infinity
gScore[start] := 0
// fScore估值集合
fScore := map with default value of Infinity
fScore[start] := heuristic_cost_estimate(start, goal)
while openSet is not empty
//取出F值最小的節點設為當前點
current := the node in openSet having the lowest fScore[] value
//當前點為目標點，跳出循環返回路徑
if current = goal
return reconstruct_path(cameFrom, current)
openSet.Remove(current)
closedSet.Add(current)
for each neighbor of current
if neighbor in closedSet
continue // 忽略關閉列表中的節點
// tentative_gScore作為新路徑的gScore
tentative_gScore := gScore[current] + dist_between(current, neighbor)
if neighbor not in openSet
openSet.Add(neighbor)
else if tentative_gScore >= gScore[neighbor]
continue //新gScore>=原gScore，則按照原路徑
// 否則選擇gScore較小的新路徑，并更新G值與F值。同時更新節點的父子關系。
cameFrom[neighbor] := current
gScore[neighbor] := tentative_gScore
fScore[neighbor] := gScore[neighbor] + heuristic_cost_estimate(neighbor, goal)
return failure
//從caomeFrom中從goal點追溯到start點，取得路徑節點。
function reconstruct_path(cameFrom, current)
total_path := [current]
while current in cameFrom.Keys:
current := cameFrom[current]
total_path.append(current)
return total_path