Learning Objectives
- 把柵格地圖看成圖，用 Dijkstra 與 A* 找最短路徑
- 說明啟發函數為什麼能減少展開的格子，什麼條件下結果仍然最短
- 解釋膨脹與代價地圖：機器人有大小，路徑不能貼著牆
- 把規劃出來的路徑交給控制器實際走完
地圖怎麼變成可以搜尋的東西
機器人現在有地圖，也知道自己在教室門口。它要去走廊另一端的飲水機，中間有柱子和幾張椅子。路徑規劃要回答的問題是：走哪些格子？
做法是把地圖看成圖（graph）：每個空的格子是一個節點，相鄰的格子之間有邊，邊的權重是移動距離。上下左右是 1，斜著走是 √2（約 1.414）。這樣就變成圖論的最短路徑問題，機器人領域沒有發明新東西。
Dijkstra 演算法（1959）的想法是由近到遠一圈一圈往外走：每次都從「目前已知距離起點最近、還沒處理的格子」往外展開。第一次走到目標時，那條路就是最短的。
它的問題是完全不知道目標在哪裡，往目標的反方向也同樣認真地搜尋。
A*（Hart、Nilsson、Raphael，1968）多加一項估計：
Dijkstra 的排序依據：f(n) = g(n)
A* 的排序依據：f(n) = g(n) + h(n)
g(n)：從起點走到 n 實際花了多少
h(n)：從 n 到終點「估計」還要多少（啟發函數）
h 讓搜尋優先往目標的方向走。只要 h 不會高估真實距離，A* 找到的仍然是最短路徑。常用的 h：
| 移動方式 | 啟發函數 | 為什麼不會高估 |
|---|---|---|
| 上下左右 | 曼哈頓距離：abs(dx) + abs(dy) |
沒有牆時也至少要走這麼多步 |
| 八方向 | 對角距離：max(abs(dx), abs(dy)) + (√2 - 1) × min(abs(dx), abs(dy)) |
同上，只是斜著走比較省 |
如果 h 永遠是 0，A* 就變成 Dijkstra。
一步一步看 A* 怎麼挑下一格
一張 4 × 3 的小地圖，起點和終點都在第一列，中間被牆隔開。注意 heapq 每次拿出 f 最小的格子：一開始 h 把搜尋往右引，碰到牆之後 f 變大，才改走下面繞過去。
動手寫：A*
四方向移動，每步代價 1，所以啟發函數用曼哈頓距離。補上 h 和放進佇列的 f。程式會印出路徑和展開的格子數。
機器人不是一個點
上面的路徑常常貼著牆角走。把機器人當成一個點時這沒問題，但真實機器人有半徑，中心走在牆邊的格子上，車身就撞到牆了。
常見的作法是膨脹（inflation）：與其在搜尋時檢查「車身會不會撞」，不如先把牆加厚一個機器人半徑，再把機器人當成一個點來規劃。這樣規劃器不用知道機器人的形狀（以圓形機器人來說）。
膨脹有一個馬上會碰到的副作用：太窄的門會被封起來。這不是錯誤，而是地圖在告訴你「這台機器人過不去」。
但只把牆加厚還不夠。貼著膨脹區邊緣走理論上不會撞，實際上定位和控制都有誤差，稍微偏一點就撞了。所以除了「不能走」的區域，再加一層代價：越靠近牆，走起來越貴。規劃器會自己權衡繞遠路還是走靠近牆的地方。這種每格記一個代價的地圖叫代價地圖（costmap）。
ROS 2 的 Nav2 把代價地圖分成很多層（靜態地圖、感測器看到的障礙物、膨脹）再疊在一起，全域規劃器與局部控制器各有一張。它的膨脹是依離障礙物的距離呈指數衰減；下面的小工具為了好讀，用線性衰減。
規劃好了，還要走得到
規劃器只產出一串格子，沒有下任何速度指令。把路徑變成 /cmd_vel 是控制器的工作，第 9 課已經寫過：依序朝著下一個路點轉向、前進，到了就換下一個。
分工要記清楚：
- 規劃器：知道地圖，不管馬達，通常幾秒才重算一次
- 控制器：不管全局，讀最新的感測器與位置，每秒下很多次指令
下面的節點把兩者接起來：開機時用 A* 算一次路徑，之後用 P 控制沿著路點走。藍色虛線看不到，但左邊世界的藍線就是實際走過的路。
確認一下
八方向移動的地圖，啟發函數用曼哈頓距離 abs(dx) + abs(dy)，會發生什麼事？
A* 的 h 永遠回傳 0，和 Dijkstra 比起來如何？
膨脹半徑設定成機器人真實半徑，一點都沒有多留。真實世界會有什麼問題？
機器人開機時規劃一次路徑，之後走廊上有人放了一台推車。只有規劃器和路徑跟隨的系統會怎樣？
一張 1000 × 1000 格的地圖，用 A* 規劃一條穿越整張圖的路。怎樣最可能減少計算量？
重點整理
- 柵格地圖可以看成圖，每個空格是節點。規劃就是最短路徑問題。
- Dijkstra 由近到遠全方向搜尋；A* 用
f = g + h優先往目標走。h不高估就保證最短。 h看不見牆，遇到死路 A* 省得比較少；權重大於 1 換到速度，放棄最短保證。- 機器人有大小：先膨脹牆，再把機器人當成點；代價地圖讓路徑自己離牆遠一點。
- 找不到路徑時要停下來回報，不要放寬安全邊界硬走。
下一課處理規劃器看不到的東西：路上突然出現的障礙物。