在網格上尋找最短路徑

把自由空間變成一張圖

在機器人規劃路線之前，它需要一種方式來描述「自己可能去到的所有位置」。一個清晰的做法是給世界鋪上一張網格，把地面切成一個個方格。每個方格要麼是空的（機器人可以站在那裡），要麼被障礙物擋住——這種把自由空間與障礙空間區分開來的劃分，是規劃的基礎。空的方格就成了我們被允許造訪的地點。

現在，把每個空方格與它相鄰的空方格連起來——也就是正北、正南、正東、正西的方格（通常還包括四個對角方格）。每一條連接都是一條你可以行走的「邊」，每條邊都有一個代價：通常就是跨過它所走的距離。你所搭建出來的，就是一張圖——一堆由線（移動）連接起來的點（方格）。此時，尋找路線就和在地鐵線路圖上規劃行程完全一樣：從一站跳到下一站，直到抵達你的目的站。

這種重新表述之所以強大，是因為電腦科學家已經研究圖搜尋數十年了。一旦世界變成一張圖，我們就能借用那些經過充分研究、可證明正確的演算法，而不必發明某種脆弱的東西。網格還悄悄地照顧到了機器人的身體：只要把機器人放不進去的方格都標記為被佔據，搜尋就只會傳回那些通過碰撞檢測的路徑。

Dijkstra：均勻蔓延的洪水

Dijkstra 演算法是尋找最短路線時那種耐心而仔細的方法。想像把水倒在起點方格上。水向四周蔓延，總是優先填滿最近的、還乾著的方格。因為它總是從「到達代價最低的邊界」開始擴張，所以當洪水觸及終點的那一刻，你就知道自己是沿著可能的最短路線抵達的——沒有任何更便宜的走法能更早到達那裡。

給每個方格一個「從起點到此的最低代價」的累計值。起點為 0，其餘所有方格初始為無窮大（尚未到達）。
反覆挑出累計值最小的、尚未定下來的方格——也就是最便宜的邊界——並將它「定下來」；它的代價從此確定。
對每個鄰居，檢查「經由當前方格到達它」是否比它現有的累計值更便宜。如果是，就調低它的累計值，並記住「你是從當前方格過去的」。
當終點被定下來時停止。然後從終點沿著「我是從哪過來的」連結一路倒著走回起點，就能讀出整條路徑。

A*：有方向感的 Dijkstra

Dijkstra 的「盲目」——哪怕是背離終點的方向也均勻蔓延——在你已經大致知道終點在哪裡時就顯得很浪費。A* 搜尋透過給洪水一種方向感來解決這個問題。對每個方格，它額外加上一個啟發值：對「還剩多少距離才到終點」的一個廉價猜測，通常就是直線（「烏鴉飛過去」）的距離。

Dijkstra 僅按「已花費的代價」給方格排序，而 A* 則按「已花費的代價」加上「對剩餘代價的啟發式猜測」來排序。用一條簡短的公式來說，它按 f = g + h 給每個方格打分：g 是從起點已經真實走過的距離，h 是估計還剩下的距離。朝著終點方向的方格 f 值更小，會被優先探索，於是搜尋就強烈地偏向正確的方向，把身後大半張地圖都忽略掉。

priority of a cell  f(cell) = g(cell) + h(cell)
  g(cell) = real cost from start to this cell (known)
  h(cell) = guessed cost from this cell to goal (estimate)

Dijkstra is just A* with h = 0 (no sense of direction).

A* 按 f = g + h 給方格排序；把 h 設為零，它就退回成 Dijkstra。

從課本網格到機器人地圖

真實的機器人不會被人遞上一張整齊的網格——它要從自己的感測器裡把網格建出來。常見的產物是一張佔據柵格地圖：一張方格網，每個格子的數值代表機器人對「這個格子被佔據、空閒，還是未知」的信念。這張信念地圖，正是 Dijkstra 與 A* 執行所需的那張「空閒對被佔據」的圖景。

在規劃之前，機器人通常會把佔據柵格轉換成一張代價地圖，其中每個格子攜帶的不只是「是否被佔據」，而是「待在那裡」的分級代價。緊貼障礙物的格子即便嚴格來說是空閒的，也會被「膨脹」成高代價，從而促使規劃器留出舒適的餘量，而不是擦著牆走。把這張代價地圖交給 A*，你得到的路線既短，又明智地遠離危險。

在實踐中，執行於代價地圖之上的 A*，就是機器人導航堆疊裡那個經典的全域規劃器。它在整張地圖上勾勒出大方向的路線；隨後由一個獨立的、更快的區域規劃器，把這條路線轉化成平滑的、逐刻更新的運動指令，並避開那些在地圖建好之後才冒出來的東西。這種全域規劃器與區域規劃器的分工，正是為什麼一台配送機器人既能橫穿一棟樓，又能繞開一輛經過的推車。

網格力不從心之處

網格很美妙，但它有兩個不容迴避的侷限。第一，它會模糊掉精細的幾何：一個格子要麼開要麼閉，所以一道恰好夠機器人通過的門，如果正好跨在格子邊界上就可能消失不見；而且除非事後做平滑，路徑只能鎖定在直角和 45 度的步子上。還要記住，網格規劃的是空間中的一條路線——是路徑，而非軌跡——把速度和時間留給後續階段去填補。

第二個侷限對機械臂來說是致命的。平坦的地面是一張二維網格；而一條有六個關節的機械臂，活在一個六維的位形空間裡，那裡的一個「格子」是六個關節角度的某一種組合。哪怕每個維度只切成 100 份，你也會得到 100^6——也就是一萬億——個格子。任何洪水，無論它的啟發值多麼聰明，都無法把一萬億個格子一一定下來。網格就這樣直接爆炸了。

這種爆炸，正是高維規劃器拋棄完整網格的原因。基於取樣的規劃不再造訪每一個格子，而是把隨機點撒進位形空間，只把那些被證明無碰撞的點連接起來——像快速擴展隨機樹（RRT）這樣的方法，會朝著終點生長出一條路徑，卻從不把整個空間一一列舉出來。一旦網格再也裝不進記憶體，這就是順理成章的下一步。