3.2 GuidePin 之原型設計
3.2.2 傾斜修正
國
立 政 治 大 學
‧
N a tio na
l C h engchi U ni ve rs it y
33
3.2.2 傾斜修正
1. 靜止狀態之修正
三維電子羅盤通常會搭配加速度計（G sensor）一起使用，主要可利用加速度計所 測得的物體傾角，來補償電子羅盤的數值，本研究將使用 GuidePin 內建之加速計感測器 -BMA250。在水平的情況下，下列式（3）讀取 X 與 Y 軸的輸出並帶入 arcTan（Y/X）
便可得方向角，若有傾斜角度的變異數則必須先通過三軸加速度傳感器檢測出三個軸上 重力加速度的分量，再通過式（1）與（2）計算出 Pitch 和 Roll。Pitch（Φ）定義為 x 軸 和水平面的夾角；Roll（θ）定義為 y 軸和水平面的夾角，如圖 27 所示方向皆為正方向。
運算流程如圖 28，當 MCU 接收到加速度計與電子羅盤的輸出後，透過公式算出方向角 度並作補償。公式如下（1）~（3）式。
(1) 𝑋𝐻 = 𝑋 ∗ sin 𝜑 + 𝑌 ∗ sin 𝜃 ∗ sin 𝜑 − 𝑍 ∗ cos 𝜃 ∗ sin 𝜑
(2) 𝑌𝐻 = 𝑌 ∗ cos 𝜃 + 𝑍 ∗ sin 𝜃
(3) Azimuth = arc tan𝑌𝑋𝐻
𝐻
圖 27 羅盤傾斜情況
‧ 國
立 政 治 大 學
‧
N a tio na
l C h engchi U ni ve rs it y
34
2. 行走狀態之修正
GuidePin 透過上述計算加入加速度補償後可有效減少因傾斜而產生的影響，然而若 要將感應器套用在人的移動上，則須考慮因行走帶來的晃動、任意方向的變動或是環境 的干擾皆有可能帶來雜訊，此時可使用卡爾曼濾波器（Kalman Filter）來處理突波問題。
卡爾曼濾波器是一種資料處理的演算法（optimal recursive data processing algorithm），可 從前一個結果計算預測現在的最佳結果及下一個結果，最常應用在慣性導航系統中，例 如車體運行的軌跡或四軸飛行器的平衡等，有三項重要假設：
被建模的系統是線性的（Linear Stochastic Difference equation）
影響測量的雜訊屬於白雜訊（White Gaussian Noise）
雜訊本質上是高斯分佈的（Gaussian Distribution）
本研究則是利用簡化的卡爾曼濾波器來校正加速度的值（如圖 29），其內容摘要如 下步驟：
(1) 假設現在的系統狀態是 k，首先利用上一個狀態最優的結果預測下一個狀態，
其中 A 和 B 是系統參數，U（k）是 k 時刻對系統的控制量。
X（k|k-1）=A X（k-1|k-1）+B U（k） ……….. （1）
圖 28 電子羅盤傾角修正流程
‧ 國
立 政 治 大 學
‧
N a tio na
l C h engchi U ni ve rs it y
35
(2) 此時系統結果已更新，但對應於 X（k|k-1）的斜方差（covariance）還沒更新。
其中 P（k|k-1）是 X（k|k-1）對應的斜方差，P（k-1|k-1）是 X（k-1|k-1）對應 的斜方差，A’表示 A 的轉置矩陣，Q 是系統過程的斜方差。
P（k|k-1）=A P（k-1|k-1） A’+Q ……… （2）
(3) 計算卡爾曼增益 Kg（Kalman Gain），H 是測量系統的參數，R 為測量雜訊的斜 方差。
Kg（k）= P（k|k-1） H’ / （H P（k|k-1） H’ + R） ……… （3）
(4) 更新狀態（k）的最優化估算值 X（k|k），Z（k）是 k 時刻的測量值。
X（k|k）= X（k|k-1）+Kg（k） （Z（k）-H X（k|k-1）） ……… （4）
(5) 更新 k 狀態下 X（k|k）的斜方差。
P（k|k）=（I-Kg（k） H）P（k|k-1） ……… （5）
如此將 covariance 遞迴下去就可以一直計算出現在的最佳結果，而且只需要從上一 個保存的結果推算，本研究利用簡化卡爾曼濾波器的方法來處理雜訊，可看成在同一個 系統下建模，因此令當下與跟前一時刻的加速度值相同且沒有控制量，經模擬得出測量 產生的雜訊斜方差 R=1e-5 與處理過程產生雜訊的斜方差 Q = 1e-7，代入上述算式並於 Arduino 開發環境執行以完成卡爾曼濾波器濾波方法。
‧ 國
立 政 治 大 學
‧
N a tio na
l C h engchi U ni ve rs it y
36
為了比較與使用智慧型手機取值的結果，在不手持裝置的前提下，本研究比較了手 機掛置胸前如圖 30、配戴運動臂套與使用 GuidePin 之資料，並利用 iOS 提供給之沙盒
（sandbox）取得 iPhone 本機端資料。其中使用運動臂套因行走時的手部自然擺動會造 成數據發生過大的波動，因此不採用此種方法。如圖 31 為同時使用 GuidePin 與 iPhone 走一段直線並轉彎的數據圖，可看出 GuidePin（藍）較 iPhone（紅）穩定，圖中的高峰 轉折發生在轉彎時，可發現 iPhone 反應較快約 1.5 秒，而 GuidePin 因加入濾波效果反 應較慢，但數值變化程度緩和有助於給予使用者微調方向時的提示，故後續實驗皆採用 GuidePin，並針對步行速度與採樣頻率（sample rate ）做調整，以符合視障者之行走與 轉向之速度。
圖 29 系統加入卡爾曼濾波器示意圖
‧ 國
立 政 治 大 學
‧
N a tio na
l C h engchi U ni ve rs it y
37