Learning Objectives
- 依 angle_min 與 angle_increment 找到某個方向的光達讀數
- 說明光達、超音波、紅外線各自在什麼情況下會量錯
- 比較單一讀數、區段最小值與中位數濾波的取捨
- 說明陀螺儀積分為什麼會漂移，用靜止校正降低誤差
- 用光達寫出沿牆走
感測器量到的不是世界，是一個數字
光達說前方 3 公尺沒有東西，可能真的沒有，也可能那裡是一片玻璃門。機器人只拿得到數字，所以你要知道這個數字是怎麼來的，才知道它什麼時候不可信。
光達（LiDAR） 發出雷射脈衝，等反射回來，用飛行時間（time of flight）算距離：
距離 ＝ 光速 × 來回時間 ÷ 2
要除以 2，因為光走了一趟來回。光速約每秒 3 億公尺，3 公尺的牆來回只要 20 奈秒（0.00000002 秒），所以這需要很精密的計時電路。有些低價的光達用三角測距，原理不同，近距離表現好但遠距離誤差大。旋轉鏡片讓雷射掃一圈，就得到一整圈的距離。
ROS 2 用 sensor_msgs/LaserScan 傳這一圈資料：
| 欄位 | 意思 |
|---|---|
angle_min、angle_max |
第一筆與最後一筆的角度（弧度），逆時針為正，0 是感測器的 x 軸（朝前） |
angle_increment |
相鄰兩筆差幾弧度 |
ranges |
距離（公尺），一個陣列 |
range_min、range_max |
有效範圍，超出的讀數要丟掉 |
intensities |
反射強度，依裝置而定，可能是空的 |
scan_time、time_increment |
掃一圈的時間、相鄰兩筆的時間差 |
第 i 筆的方向是：
角度 ＝ angle_min ＋ i × angle_increment
反過來，要找某個方向是第幾筆：
i ＝（角度 − angle_min）÷ angle_increment
不要寫死索引。 前面的課用 ranges[len // 2] 當正前方，那是這個模擬世界的設定（從正後方開始逆時針一圈）。真實光達的起始角由驅動程式決定，筆數也不一樣，有的從 0 開始、有的從 −π 開始，甚至安裝時裝反了。換一顆光達，寫死的索引就會指向別的方向。
讀懂一筆 LaserScan
世界是一個房間，機器人朝向 x 軸，停在 (1.5, 1.5)；房間內部從 0.5 到 4.5 公尺。光達每 0.2 秒掃一圈，所以 0.2 秒時回呼才第一次被呼叫。
感測器在哪裡會騙你
剛才的模擬世界很老實：每一面牆都會反射。真實世界不是：
| 感測器 | 原理 | 會出錯的情況 |
|---|---|---|
| 光達 | 雷射飛行時間 | 玻璃：雷射大多穿過或鏡面反射，常常量到玻璃後面的東西，或沒有回波。鏡子：可能量到鏡中的假物體。黑色、吸光的物體：回波弱，有效距離變短，甚至沒偵測到。平面光達只掃一個高度：桌面、懸空的東西、地上的矮階都可能看不到 |
| 超音波 | 聲波飛行時間 | 波束很寬，分不出是哪個方向的物體。軟的材料（布、海綿）吸收聲波。傾斜的平面把聲波反射到別的方向，測不到回波。聲速隨氣溫變，每度差約 0.6 m/s，沒補償就有誤差 |
| 紅外線距離感測器 | 反射光的強度或三角測距 | 陽光含大量紅外線，戶外容易失效。顏色與材質影響反射，深色物體表現較差。太近時讀數不可靠 |
超音波的時間尺度比光達慢得多：聲速約 343 m/s（20°C），1 公尺的牆來回要 5.8 毫秒。
所以機器人常常裝不只一種感測器：光達看不到玻璃，超音波看得到；超音波分不清方向，光達分得清。用兩種原理不同的感測器，它們同時出錯的情況就少很多。
雜訊與濾波。 就算沒有這些狀況，每次讀數還是會有隨機誤差，偶爾還有離譜的尖峰（比如灰塵反射）。處理方式各有代價：
| 方法 | 好處 | 代價 |
|---|---|---|
| 單一讀數 | 最快、沒有延遲 | 一個尖峰就誤判 |
| 區段最小值 | 不會漏掉旁邊的東西，保守 | 任何一筆雜訊都會讓它變小，機器人容易無故停下來 |
| 時間上的中位數濾波 | 尖峰直接被丟掉 | 多了延遲：取 5 筆就要等約 2 筆的時間才反應 |
安全相關的判斷，寧可保守：把東西誤判成障礙只是多停一下，把障礙當成沒有就撞上去了。但太常誤停，使用者也會想把保護關掉，這是真實的取捨。
這裡和真實的差異。 模擬世界的光達是理想射線，所有牆都會反射，
noise.scan只加高斯雜訊，也沒有尖峰與斷線；玻璃、黑色物體在這個模擬裡都不存在。
實作題：中位數濾波
光達正前方連續 10 次讀數：第 4 筆是雜訊尖峰（0.12），第 7 筆沒有收到回波（inf）。補上 median 和 filtered：取最後 5 筆、略過 inf、算中位數。
每一行印出原始值、濾波值，以及依各自判斷的「停」或「走」（小於 0.5 就停）。看原始讀數會在哪一次誤停，濾波之後又如何。偶數筆時取中間兩筆的平均，並四捨五入到小數第 3 位。
IMU：不看外面也知道自己怎麼動
慣性測量單元（IMU） 不看外界，只感覺自己的運動，通常包含：
| 感測器 | 量什麼 | 主要問題 |
|---|---|---|
| 陀螺儀（gyroscope） | 旋轉速度（rad/s） | 有偏差（bias）：靜止時也不是 0。積分成角度時，誤差隨時間一直增加（漂移） |
| 加速度計（accelerometer） | 加速度（m/s²），包含重力 | 靜止時 z 軸讀到約 ＋9.8（REP-145：機體 z 軸朝上時讀到 ＋g）。可以用來知道哪邊是下，但要算移動距離得先扣掉重力，再積分兩次，誤差長得更快 |
| 磁力計（magnetometer） | 磁場，可推得方位 | 馬達、鋼筋、電線都會干擾，室內特別不可靠 |
用數字感受一下漂移。陀螺儀偏差 0.03 rad/s，積分之後朝向的誤差是：
誤差 ＝ 偏差 × 時間 ＝ 0.03 × 20 秒 ＝ 0.6 弧度，約 34°
只過 20 秒，方向就差了三分之一個直角。加速度計更嚴重：偏差 0.01 m/s² 積分兩次，誤差 ＝ ½ × 0.01 × t²，60 秒就差 18 公尺。
所以 IMU 擅長的是短時間內的快速變化（機器人被撞了、正在轉彎），長時間要靠別的東西校正。里程計、光達定位負責長期準確，下一課的卡爾曼濾波就是把兩者合起來的方法。
最簡單也最常用的校正：開機時靜止幾秒，把陀螺儀的平均讀數當成偏差扣掉。代價是開機時機器人不能動，而且偏差會隨溫度改變，不能一勞永逸。
sensor_msgs/Imu 有 orientation（四元數）、angular_velocity、linear_acceleration，各有一個共變異數陣列。如果裝置不提供朝向，規定是把 orientation_covariance 的第一個元素設成 −1，用的人要檢查。
這裡和真實的差異。 模擬世界的
/imu只有orientation與angular_velocity.z，直接來自真實朝向與速度，沒有偏差，linear_acceleration全部是 0（沒有重力）。下一個練習用程式加上固定偏差GYRO_BIAS，才看得到漂移；真實偏差不是固定值。
一步一步看陀螺儀漂移
機器人完全靜止，所以真實角速度一直是 0。看偏差怎麼變成角度的誤差，以及扣掉估計的偏差之後有什麼不同。
沿牆走
接著用光達完成一個真正的行為：沿牆走（wall following）。機器人保持和右邊的牆固定距離往前走，碰到前方有牆就左轉。掃地機器人沿著牆邊打掃就是這個行為。
這是第 9 課 P 控制的應用：誤差是「想要的距離 − 量到的距離」，指令是旋轉速度。但這裡有一個新問題：旋轉速度改變的是朝向，朝向再改變距離，中間隔了一層，這和第 9 課直接控制朝向不一樣，所以只用 P 很容易左右蛇行。距離的變化率（D 項）在這裡等於告訴控制器「你現在正斜著靠近牆」，蛇行就能壓下來。
程式還有一個安全設計：右邊看不到牆（inf）時不是繼續走，而是停下來。感測器沒有資料時，不要假裝有。
確認一下
光達 angle_min ＝ 0、angle_increment ＝ 0.5°、一圈 720 筆，正左方（90°）是第幾筆？
機器人在大樓走廊用平面光達導航，前方是一扇落地玻璃門。最可能發生什麼？
程式用 min(msg.ranges) 當「最近的障礙物距離」。光達偶爾有一筆讀數是 0.0（代表無效）。會怎樣？
機器人靜止放在平面上，z 軸朝上。真實 IMU 的 linear_acceleration.z 約是多少？
陀螺儀偏差 0.01 rad/s，沒有校正，只靠積分算朝向。10 分鐘後朝向誤差大約多少？
光達突然停止發布 /scan，程式手上的 self.front 還是最後一次的值 3.0。合理的設計是？
重點整理
- 光達用飛行時間算距離，
ranges第 i 筆的方向是angle_min ＋ i × angle_increment。不要寫死索引。 - 一條射線只代表一個方向，機器人有寬度，要看區段。
- 玻璃、鏡子、黑色物體騙光達；軟材料、斜面騙超音波；陽光騙紅外線。不同原理的感測器可以互補。
- 單一讀數快但容易誤判，區段最小值保守，中位數濾波去尖峰但有延遲。
- 陀螺儀積分會漂移：誤差 ＝ 偏差 × 時間。加速度計含重力。磁力計怕干擾。靜止校正有用但不是永久的。
- 感測器沒有更新時，舊資料不可信，預設動作是停下來。
- 模擬的光達是理想射線，模擬的 IMU 沒有偏差與重力，這些差異在真實機器人上都要面對。
下一課處理一個根本問題：里程計會漂移、感測器有雜訊，兩個都不準的量要怎麼合起來，得到比兩個都好的估計？這就是卡爾曼濾波。