Learning Objectives
- 說明 P、I、D 三項各自解決什麼問題、各自有什麼副作用
- 用公式預測只用 P 時的穩態誤差，再用實際軌跡驗證
- 親手調參：讓機器人準確轉到指定方向、走到目標
- 處理角度誤差的 ±180° 與積分飽和，並設定速度上限
只會全速和停下來是不夠的
要機器人轉到 90°，最直接的寫法是：還沒到就全速轉，到了就停。第 8 課的正方形就是這樣，結果每個角都多轉一點，因為看到「到了」的時候已經衝過頭。把指令想成開車：快到路口才急踩煞車，不會有人這樣開；通常是距離越近，踩得越輕。
這個想法寫成式子就是 P 控制（比例控制）：
誤差 e ＝ 目標 − 目前的值
指令 u ＝ Kp × e
差得多就給大指令，差得少就給小指令，差 0 就不動。Kp 是你要選的數字。
問題來了：真實的機器人常有固定的干擾，例如一邊輪子阻力比較大，就算指令是 0，它也會慢慢偏。假設偏差是 d（rad/s），要讓朝向停下來不動，指令必須剛好抵銷它：Kp × e ＋ d ＝ 0，所以
穩態誤差 e ＝ −d ÷ Kp
只用 P，永遠差一點；Kp 越大差越少，但不會是 0。因為誤差如果是 0，指令也是 0，就沒有東西抵銷偏差了。下面用數字走一次。
只用 P 會停在哪裡
目標是 1.0，Kp ＝ 2，固定偏差 0.15。公式預測穩態誤差是 −0.15 ÷ 2 ＝ −0.075，也就是 x 會停在 1.075。看 x 是不是真的停在那裡。
P、I、D 三項在做什麼
完整的 PID 控制器是三項相加：
u ＝ Kp × e ＋ Ki × （e 的累加） ＋ Kd × （e 的變化率）
| 項 | 看什麼 | 解決什麼 | 太大會怎樣 |
|---|---|---|---|
| P（比例） | 現在差多少 | 主要的修正力道 | 超過目標、來回振盪 |
| I（積分） | 過去累積差多少 | 消除穩態誤差：只要還有誤差，I 就一直累加，直到指令足以抵銷干擾 | 反應變慢、超越變大；輸出飽和時累積過頭 |
| D（微分） | 誤差變化得多快 | 快到目標時提前收力，抑制超越 | 放大雜訊 |
為什麼會超越？ 機器人（和馬達）有慣性：就算誤差已經是 0、指令降到 0，轉速也要一段時間才會停下來。P 不知道這件事，D 會看到「誤差正在快速變小」而提前減速。
積分飽和（windup）。 轉一個大角度時，指令早就碰到速度上限（比如 1.5 rad/s），機器人已經盡全力了，I 卻還在累加。等到達目標時，累積的 I 非常大，要過頭很久才退得掉，造成大超越。常見的解法是輸出飽和時暫停累加。
下一個練習的機器人多接了一個節點 motor_sim：你的指令發到 /cmd_vel_raw，它加上馬達的反應時間（0.4 秒）和固定偏差（0.15 rad/s）再發到 /cmd_vel。
為什麼要多這個節點。 這條軌道的模擬世界沒有慣性，指令當下就變成速度，也沒有干擾，那樣 P 就幾乎夠用了，看不出 I 和 D 的作用。
motor_sim是課程寫的簡化模型，反應時間與偏差都是固定值；真實的馬達還有摩擦力、死區、背隙和隨負載改變的反應，所以這裡調出來的數字不能直接拿去用，但調參的邏輯相同。
一步一步看 PID 三項的數字
看每一步 P、I、D 各貢獻多少。一開始 P 最大；接近目標時 D 變成負的，把指令往回拉；I 慢慢累積。這裡的物理和前面的 motor_sim 相同（反應時間 0.4 秒、偏差 0.15）。
調參的順序與角度的陷阱
實際調參的常見順序，也是你剛才做的順序：
- Ki、Kd 先設 0，加大 Kp，直到響應夠快、開始有明顯超越或振盪，再往回退一些。
- 加 Kd 壓低超越。感測器有雜訊時，D 會把雜訊放大，真實系統常要先濾波，或只對量測值微分（這樣目標突然改變時也不會有一個大尖峰）。
- 有穩態誤差才加 Ki，從小的開始，並做好防飽和。沒有固定干擾的系統，I 常常只有壞處。
每改一次只動一個參數。還有兩件事，在機器人上幾乎每次都會碰到：
角度誤差要處理 ±180°。 朝向 170° 要轉到 −170°，相減得到 −340°，機器人會往反方向繞一大圈；其實只差 20°。第 2 課的做法：
e = math.atan2(math.sin(target - yaw), math.cos(target - yaw))
速度上限是安全設計。 P 控制的指令和誤差成正比，目標很遠時指令就很大。不要指望底層幫你擋：要在控制器裡限制，而且要依環境決定（走廊有人時走慢一點）。另外，感測器沒有更新時（/odom 超過一段時間沒有新資料），控制器手上的是舊值，正確的預設動作是停下來。
下一個練習沒有 motor_sim，指令直接到模擬世界（沒有慣性），Kp 可以設得比較大也不會振盪；真實的機器人不會這麼好調。走到目標要控制兩個量：距離決定前進速度，朝向誤差決定旋轉速度。
實作題：寫一個 PID 類別
補上 step，順序要照這樣（順序不同，數字會差一點）：
- 算變化率
d：prev_error是None時為 0，否則是（error − prev_error）÷ dt；然後更新prev_error。 - 用目前的
integral算u ＝ kp × error ＋ ki × integral ＋ kd × d。 abs(u)小於limit才把error × dt加進integral（防飽和）。- 回傳限制在 ±limit 之內的
u。
印出來的是（最高點, 最後的值），目標是 1.0。對照三種設定，看哪一種有穩態誤差、哪一種沒有。
確認一下
固定偏差 0.2 rad/s，只用 P，Kp ＝ 4。穩態時朝向和目標差多少？
機器人接近目標時總是衝過頭再回來，穩態誤差倒是很小。最優先試什麼？
控制器的 Kd 很大，感測器有一點雜訊。會發生什麼？
目標朝向 −170°，目前朝向 170°，程式寫 e = target − yaw、Kp 為正。機器人會怎麼轉？
機器人碰到速度上限後，積分項繼續累加，抵達目標時會發生什麼？
重點整理
- 誤差 ＝ 目標 − 目前的值。P 看現在，I 看累積，D 看變化趨勢。
- 只用 P 且有固定干擾時，穩態誤差 ＝ 干擾 ÷ Kp，可以預先算出來。
- Kp 太大造成超越與振盪；D 抑制超越但放大雜訊；I 消除穩態誤差，但要防飽和。
- 調參順序：先 P，再 D，有穩態誤差才加 I，一次改一個。
- 角度誤差要用 atan2(sin, cos) 收到 ±180°。
- 速度上限寫在控制器裡，感測器沒有更新就停下來。
- 這課的馬達模型是簡化的，真實機器人要用自己的資料重新調。
下一課換一種機器人：手臂沒有輪子，它的問題是關節角度和手的位置怎麼互相換算。