感測器原理

四軸飛行器上面常會使用一些感測器做為回授，來取得飛行器的姿態、航向或是高度等資訊，常見有陀螺儀、加速度計、磁力計以及氣壓計這四種，由於微機電系統（Micro Electro Mechanical System, MEMS） 技術的發達，把機械系統縮小到微米（10E-6）或是奈米（10E-9）等級，使的這些感測器得以封裝起來，變成小的 IC。

慣性測量單元（Inertial Measurement Unit, IMU）捕捉物體運動的資訊、測量物體姿態的一種電子元件，又稱為慣性感測器，主要由陀螺儀（Gyroscope）與加速度計（Accelerometer）組成，部分的應用中有時也會加入磁力計（Magnetometer）與氣壓計（Barometer）來輔助估算相關訊息，經常使用在載具的導航上面。

在四軸上面四種感測元件主要的功能：

• 陀螺儀 → 通常做為主要姿態的計算，優點是反應快，但缺點是誤差容易累積
• 加速度計 → 可以透過重力加速度來校正陀螺儀，運動加速度部分也可以用來計算位移與速度等
• 磁力計 → 同樣也是輔助、校正陀螺儀，解決重力加速度無法檢測到水平角度旋轉的問題
• 氣壓計 → 計算飛行器高度 　

上述四種感測器的種類與型號眾多，但依輸出可以分成數位與類比兩種，數位輸出通常使用像是 SPI、I2C 等通訊介面來獲取感測器資料，電路處理上較簡單，功能較多，可能包含濾波器等，而另一種則是類比的輸出，輸出的資料大小會與電壓成正比，會把原始感測到的資訊直接輸出，雖然原始，但比較靈活，通常使用 ADC 來讀取電壓資訊。

在感測器選型時，常常會看到一軸陀螺儀、三軸加速度計等等，其軸數表示為可以感測到物理量的分量，沒有方向的物理量像是溫度、氣壓等，都是單軸就可以得到完整的資訊，但有方向的物理量像是角速度、加速度等，會將物理量投影到軸上，投影就像是把一個建築物（三維）畫在紙（二維）上一樣，在紙上你看不到建築物的後面，但實際上你可以走到建築物後面看，所以軸越多，可以感測到的資訊就越完整。

一直生存在一維空間的生物，就像活在線上，他們只知道前後走，沒有左右的概念，一直生存在二維空間的生物，就像活在紙上，只知道前後左右走，沒有上下的概念，一直生存在三維空間的生物，不只可以前後左右走，還可以上下移動，能做的、能看到的東西都比一維、二維生物來的多，那四維空間的生物會看到甚麼？能做甚麼？這我也不知道，畢竟大部分的人都已經習慣三維空間了，要往高維度的空間去思考是困難的。

在說明之前，我們先定義兩個座標系統，世界座標系G以及載體座標系L，世界座標系為地球座標，載體坐標系對準載具，感測器因為放置在載具上面，所以讀取的數值都是相對於載體座標系，除非不同或是需要注意才會再行標示。

這裡以三軸陀螺儀、三軸加速度計、三軸磁力計和氣壓計做說明，首先要先了解三軸之間的關係，假設有三個向量 X、Y、Z，其方向分別為三個軸的方向，那這三個向量會互相垂直、正交（Orthogonality），也就像是 X dot product Y = 0 或 X cross product Y = Z，實心點垂直於螢幕出來，叉叉點垂直於螢幕進入，可以用右手定則來了解。

陀螺儀（Gyroscope or Gyro）

陀螺儀是測量角速度的元件，陀螺儀有短時間準確、靈敏的特性，但用來計算角度時，長時間下的誤差累積會使其變的不可靠，通常我們以 dps（degree per second）來表示角速度，角速度與速度不同，角速度是單位時間內旋轉的角度，而速度則是單位時間內移動的距離，所以若是沒有旋轉是沒有角速度的，若只對 X 軸旋轉，則 Y 軸與 Z 軸的輸出為 0，X 軸的輸出為旋轉的角速度。

下面一張圖是角度、角速度、角加速度的關係，紅線是角速度，綠線是角加速度，角速度的一階微分，藍線是角度，角速度的一階積分，所以只要知道取樣時間以及角速度，就可以對其作積分得到當下的角度了。

但是實並非如此，原因就在於感測器都會有誤差與雜訊，如何把誤差與噪音雜訊給濾除掉也是一門學問，下面的數學式子表示從感測器讀到的角速度 w_measure 會等於矩陣 R_gyro 乘上理想的角速度 w_ideal 加上偏移 w_bias 與雜訊 n_noise，矩陣 R_gyro 包含未對準誤差（Misalignment）以及單位轉換（dps/LBS），若感測器與載具完全對準的情況（大部份安裝不准等情況會造成未對準誤差），矩陣 R_gyro 會是一個對角矩陣（Diagonal Matrix），偏移 w_bias、雜訊 n_noise 則由溫度、感測器內部等干擾引起。

理想的角速度 w_ideal 是我們想要的，所以校正的動作其實就是把 R_gyro、w_bias 與 n_noise 求出來並去除。

加速度計（Accelerometer or G-Sensor）

加速度計是測量加速度的元件，除了運動加速度外，也包含地球質量所產生的重力加速度，也就是當感測器自由落體時，運動加速度與重力加速度會剛好抵銷，有輸出都會變成 0，通常以 g 或是 m/s^2 來表示，因為可以感測到重力加速度，所以在有重力的環境中加速度計也常用來做傾斜計，透過重力在三軸上的投影來計算角度。

加速度計常常會搭配陀螺儀來應用，因為重力加速度並不會隨時間快速變化，長時間具有可靠性，所以通常透過此特性來較正陀螺儀，防止誤差的累積。

下圖是在靜止下（僅存在 1g 的重力加速度），不同角度的加速度計，角度 1～8 的 Z 軸因為垂直於重力加速度，所以皆為 0g，

角度 1 （ 0 deg）的輸出（0, +g, 0）
角度 2 （ 45 deg）的輸出（+0.707g, +0.707g, 0）
角度 3 （ 90 deg）的輸出（+g, 0, 0）
角度 4 （135 deg）的輸出（+0.707g, -0.707g, 0）
角度 5 （180 deg）的輸出（0, -g, 0）
角度 6 （225 deg）的輸出（-0.707g, -0.707g, 0）
角度 7 （270 deg）的輸出（-g, 0, 0）
角度 8 （315 deg）的輸出（-0.707g, +0.707g, 0）

歸納出 → X = g_sin(theta), Y = g_cos(theta) → theta = atan(X / Y)

表示在 Z 軸垂直於重力時，輸出的 X, Y 值相除，並取 atan 則可以得到傾斜角度，但需要注意的，這僅限於靜止下，當有運動加速度的影響，得到的角度會是不準確的。

下面是一個加速度計的簡單數學模型，與陀螺儀類似（其實大家都長得差不多），加速度計的輸出 a_measure 會等於矩陣 K_accel 乘上理想的加速度 a_ideal 加上偏移 a_bias 與雜訊 n_noise，理想的加速度 a_ideal 包含運動加速度 a_motion 以及重力加速度 a_gravity，矩陣 K_accel 包含未對準的誤差以及單位的轉換（g/LBS），若感測器與載具完全對準的情況，矩陣 K_accel 也是一個對角矩陣，偏移 a_bias、雜訊 n_noise 則也會由溫度、感測器內部等干擾引起。

磁力計（Magnetometer or Compass）

磁力計或電子羅盤是用來測量磁場的元件，通常以高斯（Gauss）或特斯拉（Tesla）來表示磁場大小，因為地球本身存在磁場，所以磁力計可以藉由磁場在三軸上的投影來計算出航向角度，但事實上環境中的磁場不僅僅只有地球磁場，還包含了許多的干擾源，依特性可以分成硬磁（Hard Iron）干擾與軟磁（Soft Iron）干擾，硬磁干擾是像永久磁鐵、電池這些指固定強度的干擾，軟磁干擾則是指像電磁鐵、電量變化等，當磁力來源消失時，磁力會變弱，會改變磁力線強度或方向的干擾。

下圖是將以磁力計 X 軸與 Y 軸為座標在不同影響下畫出來的圖，紅色是理想、不受干擾的磁場，是一個圓心在原點的圓形，綠色是受到硬磁干擾的磁場， 也是圓形的磁場，但圓心會編離中心，深藍色是受到軟磁干擾的磁場，會讓圓形變形成橢圓形，但圓心仍在中心，淺藍色是同時受到硬磁與軟磁干擾的形況，偏離中心的橢圓形，這也是大部分磁力計測到的圖形，如果不校正成圓形的話，在行像的計算就會產生誤差，導致問題出現。

下面是一個磁力計的簡單數學模型，磁力的輸出 h_measure 會等於矩陣 M_mag 乘上理想的磁場 h_ideal 加上偏移 h_bias 與雜訊 n_noise，矩陣 M_mag 包含未對準的準誤、單位的轉換（gauss/LBS）、硬磁干擾、軟磁干擾，偏移 h_bias、雜訊 n_noise 則也會由溫度、感測器內部等干擾引起

氣壓計（Barometer）

氣壓計是用來測量氣壓的元件，通常使用帕（Pa）或百帕（hPa）來表示，一個大氣壓力大約是 1013.25 hPa，大氣壓會隨著高度的提升而下降，其關係為每上升 9 公尺，大氣壓力降低 100 Pa，透過此關係可以使用氣壓計計算出目前的海拔高度，以目前 MEMS 氣壓計可以做到 1 Pa RMS 的高解析度情況下，大約可以檢測到 9 公分的高度變化，但這都在是比較理想的情況下，實際上還會有空氣的流動、溫度與濕度的影響，使得計算、測量的誤差產生。

詳細的感測器校正方法之後會再說明，這一章只先說明感測器的一些原理與模型。

下面這篇文章值得參考：

A Guide To using IMU (Accelerometer and Gyroscope Devices) in Embedded Applications
http://www.starlino.com/imu_guide.html