局部避障與導航

路上突然有東西

機器人沿著規劃好的路徑走到走廊中間，有人推著一台推車走出來。地圖裡沒有這台推車，全域路徑也沒有考慮它。

重新規劃整條路徑可以解決嗎？可以，但太慢：全域規劃要處理整張地圖，機器人以每秒半公尺前進，又要在幾十毫秒內做出反應。而且推車還在動，算好的路徑馬上又過時了。

所以導航系統通常分兩層：

兩者互補：全域規劃給方向，局部控制負責安全地往那個方向走。這一課的重點在局部控制。

動態視窗法：在做得到的速度裡選一個最好的

動態視窗法（Dynamic Window Approach，DWA）由 Fox、Burgard 與 Thrun 在 1997 年提出。它的特別之處是直接在速度空間裡搜尋，而不是在地圖上找路。差速機器人的指令只有兩個數字：v（前進速度）和 ω（轉速），所以問題變成：這一刻要下哪一組 (v, ω)？

每個控制週期做四件事：

1. 動態視窗：機器人有慣性，下一個週期能達到的速度只有目前速度附近的一小塊。在這個範圍裡平均取樣，例如 5 種 v × 11 種 ω ＝ 55 組。
2. 預測：假設每一組速度維持不變，往前模擬 1～2 秒，得到一條弧線。
3. 刪除不安全的：弧線會碰到障礙物的刪掉；以這個速度來不及煞住的也刪掉，也就是 v² / (2a) 要小於到障礙物的距離。
4. 評分：剩下的每一條打分數，選最高的，只執行一個週期，下一個週期整個重來。

總分 = α × heading + β × clearance + γ × speed

heading    預測終點的朝向有多對準目標
clearance  預測軌跡離最近的障礙物有多遠
speed      走得多快

三個權重就是機器人的「個性」：α 大的急著對準目標，β 大的膽小、離東西遠遠的，γ 大的不想慢下來。

如果沒有任何一組速度是安全的，就下 v = 0。機器人的控制器要能接受「現在最好的選擇是不動」，這不是失敗。

下面的工具用的是教學上簡化的評分：三項都換算到 0～1 直接相加。原始論文與 Nav2 的控制器細節都不完全相同，想法是一樣的。

用光達寫一個最簡單的避障

動態視窗法需要知道障礙物在哪裡，真實機器人的這份資訊來自光達。下面的節點比動態視窗法簡單得多，叫做反應式避障：不預測，看到前面太近就轉彎。

寫這種程式最常見的錯，是只看正前方那一條射線：

front = msg.ranges[len(msg.ranges) // 2]    # 只有一條線

機器人有寬度，一條射線只知道中心線上的狀況。障礙物偏一點點，射線完全打不到，車身卻會撞上去。所以要看一個扇形：取一個角度範圍裡的最小值，範圍至少要涵蓋車身寬度。程式裡的 sector_min 就是這樣做，用 angle_min 和 angle_increment 把索引換成角度，這樣換一台光達也不用改。

Nav2：真實的導航系統怎麼組起來

到這裡為止你親手寫過地圖、定位、規劃、控制。ROS 2 的 Nav2 把這些做成可以替換的元件。以下依官方文件整理：

          目標位姿（NavigateToPose action）
                     │
                     ▼
              BT Navigator（行為樹）
       ┌────────────┼──────────────┐
       ▼            ▼              ▼
  Planner Server  Controller Server  Behavior Server
  算全域路徑       算速度指令         恢復行為：轉圈、後退、等待
       │            │
  全域代價地圖     局部代價地圖

• BT Navigator：用行為樹（behavior tree）決定流程。行為樹把「規劃，然後跟隨，失敗了就試恢復，再失敗就放棄」寫成一棵可以改的樹，而不是寫死在程式裡。
• Planner Server：算路徑。演算法是外掛，可以換。
• Controller Server：依路徑算速度。一樣是外掛，有 DWA 系列的 DWB，也有 MPPI、Regulated Pure Pursuit 等不同的做法。
• Behavior Server：放恢復行為。
• AMCL（第 17 課）提供位置，代價地圖（第 18 課）提供環境。

各元件之間大多用 action 溝通，所以 BT Navigator 呼叫它們和你第 6 課寫 action client 是同一件事。

卡住了怎麼辦。 局部控制會走進死路，感測器也可能暫時被擋住。導航系統需要明確的恢復行為（recovery behavior），而且順序要由輕到重：清掉代價地圖裡可能過時的障礙物、原地轉一圈重新看看、往後退一點、再重新規劃。全部試完還不行就回報失敗，讓上層決定。這比無限重試安全：一台一直試圖硬擠過去的機器人，對旁邊的人來說很可怕。

外部程式要叫機器人去某個地方，用的是 NavigateToPose action（nav2_msgs/action/NavigateToPose）。主要欄位節錄如下，完整定義還有錯誤代碼常數與路徑追蹤誤差，以官方為準：

# Goal
geometry_msgs/PoseStamped pose     要去的位姿，frame_id 通常是 map
string behavior_tree               可以指定行為樹，留空用預設
---
# Result
uint16 error_code                  為什麼失敗
string error_msg
---
# Feedback
geometry_msgs/PoseStamped current_pose
builtin_interfaces/Duration navigation_time
builtin_interfaces/Duration estimated_time_remaining
int16 number_of_recoveries         已經試過幾次恢復
float32 distance_remaining

呼叫的寫法和第 6 課相同。下面這段不能在這裡執行，瀏覽器的模擬環境沒有 Nav2，只有這個訊息的定義：

from rclpy.action import ActionClient
from nav2_msgs.action import NavigateToPose

client = ActionClient(node, NavigateToPose, 'navigate_to_pose')
client.wait_for_server()
goal = NavigateToPose.Goal()
goal.pose.header.frame_id = 'map'
goal.pose.pose.position.x = 3.0
goal.pose.pose.orientation.w = 1.0
future = client.send_goal_async(goal, feedback_callback=on_feedback)

留意回饋裡的 number_of_recoveries：數字一直增加，代表機器人一直遇到麻煩，上層系統應該要知道。

重點整理

• 全域規劃看整張地圖、慢；局部控制看附近、快。局部控制目光短淺，靠全域路徑才不會走進死路。
• 動態視窗法：在下一刻做得到的速度裡取樣，預測，刪掉不安全的（包括煞不住的），評分，只執行一個週期。
• 權重決定行為，太看重目標會卡在障礙物前面。沒有安全的速度就停下來。
• 感測要涵蓋車身，不能只看一條射線。
• Nav2 由 BT Navigator、Planner、Controller、Behavior 等元件組成；外部用 NavigateToPose 下目標。卡住時有限度地恢復，用盡就回報失敗。

最後一課把這些接起來，再加上一個更難的問題：機器人看到東西的時候，該不該動。