四軸飛行器上面常會使用一些感測器做為回授,來取得飛行器的姿態、航向或是高度等資訊,常見有陀螺儀、加速度計、磁力計以及氣壓計這四種,由於微機電系統(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)

陀螺儀是測量角速度(Angular velocity)的元件,基於角動量守恆的理論而設計出來,在物體姿態運算中佔有非常重要的地位,陀螺儀具有短時間準確、靈敏的特性,但誤差會隨溫度與時間變化,長時間下姿態的計算,誤差累積會使其漸漸變的不可靠,所以需要透過其他感測器來輔助校正,通常我們以 dps(degree per second)來表示角速度,角速度與速度不同,角速度是單位時間內旋轉的角度,而速度則是單位時間內移動的距離,所以若是沒有旋轉是沒有角速度的,若只對 X 軸旋轉,則 Y 軸與 Z 軸的輸出為 0,X 軸的輸出為旋轉的角速度。

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

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

理想的角速度 y_g,c 是我們想要的,所以校正的動作其實就是把 S_g、b_g 與 n_g 求出來並去除。

加速度計(Accelerometer or G-Sensor)

加速度計是測量加速度(Acceleration)的元件,除了載體的運動加速度外,也包含地球質量所產生的重力加速度,當感測器自由落體時,運動加速度與重力加速度會互相抵銷,感測器三軸輸出合為零,通常以 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 則可以得到傾斜角度,但需要注意的,這僅限於靜止下,當有運動加速度的影響,得到的角度會是不準確的。

下面是一個加速度計的簡單數學模型,與陀螺儀類似(其實大家都長得差不多),加速度計的輸出 y_a 會等於矩陣 S_a 乘上理想的加速度 y_a,c 加上偏移 b_a 與雜訊 n_a,理想的加速度 y_a,c 包含線性加速度 a 以及重力加速度 g,矩陣 S_a 包含未對準的誤差以及單位的轉換(g/LSB),若感測器與載具完全對準的情況,矩陣 S_a 也是一個對角矩陣,偏移 b_a、雜訊 n_a 則也會由溫度、感測器內部等干擾引起。

磁力計(Magnetometer or Compass)

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

下面以二維平面為例,將磁力計X軸與Y軸經歸一化後的資料在不同影響下畫出來的圖,圖(a)為理想、不受干擾的磁場,是一個圓心在原點的圓形,表示直接獲取水平方向的夾角時不會有誤差,圖(b)是受到硬磁干擾的磁場,也是圓形分布的磁場,但整體的磁場強度會被施加一定值,導致圓心編離中心,圖(c)是受到軟磁干擾的磁場,會讓磁場方向改變產生形變,使得圓型變成橢圓,但圓心仍在中心,圖(d)是同時受到硬磁與軟磁干擾的情況,偏離中心的橢圓形,這也是大部分磁力計測到的圖形,如果在不校正成圖(a)的理想情況下,透過三角函數來計算水平方向角度就會產生誤差,誤差隨干擾程度增加,同理,應用於三維空間中,三軸磁力計輸出繪製出之圖形將會變成橢球分布,校正目的則是將橢球校正回圓球。

下面是一個磁力計的簡單數學模型,磁力的輸出 y_m 會等於軟磁干擾矩陣 S 乘上理想的磁場 y_m,c 加上硬磁干擾偏移 H 與雜訊 n_m。

氣壓計(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

上一篇 → 從零開始做四軸 (四) - 飛行器組成
下一篇 → 從零開始做四軸 (六) - 感測器校正

Comments

comments powered by Disqus