2010-05-09

以陀螺儀 (HiTechnic Gyro)推算物體的旋轉角度的實作測試


陀螺儀的功能是用來測量物體旋轉時的角速度,亦即角度位置對時間的變化率,以數學方式來表示,角速度就是角度位置對時間的微分,而積分是微分的逆運算,因此就可以使用陀螺儀所測量的角速度來推算特定時間內物體進行旋轉運動的位移角度(參考1.)

(GyroAngularDisplacement)
如何計算圖1.中的陰影面積
可以運用幾何的方式,將測量期間切割成數個連續的取樣週期,再利用梯形面積的求算公式,計算出每一期間內角度位移的近似值(參考2.),經累加所有的計算結果之後即可得出總旋轉角度的估計值。
至於取樣頻率的選定則須要適當的調整,若次數太頻繁將容易累積誤差,間隔太長則會降低對角速度變化的處理能力,而在這一次的實測中則是取
32ms,得到的結果還算可以接受。


(GyroSampleMove)

設定陀螺儀的基準值
使用陀螺儀首先須決定靜止狀態下的基準值(Zero point),用來作為計算角速度時的相對參照,由以下的角速度計算方式即可以瞭解基準值的用途:
角速度
(/) = 感應器回傳值基準值
**
回傳值範圍0 ~ 1023,當逆時針旋轉時回傳值>基準值,所以角速度為正。
由於感應器相當敏感且易受到環境的影響,所以設定穩定且足夠精確的基準值並不容易,以常用的簡單平均法(Simple-average method),可預期的將很容易受到雜訊的干擾而影響計算結果的穩定性,如此在計算位移的過程必定會造成誤差的大量累積,所以對於本次的測試將會採行兩種方式來改善基準值計算的精確性:

l   首先增加Gyro回傳值的有效位數,以降低除法運算所造成的捨位誤差,於是會將每次所讀取到的回傳值都乘以64(增加6bits)。此外,也可以使用NXT 2.0所新增的浮點運算功能,不過相較於整數的bit operation,執行效率並不好。

l   其次採用加權平均法(Weighted-average method)來消弭突發雜訊的影響,這樣能夠在計算的過程中讓基準值的變化逐漸趨於穩定而得到理想的結果。所以會以128作為權值,並使用以下的計算方式:
Step 1.
先將目前記錄的基準值減掉128分之1
Step 2.
再將讀取值乘以放大係數64之後累加至基準值

Step 3.
觀察基準值的變化若趨於穩定則結束,否則繼續Step 1~Step2

3.為設定基準值的NXC程式碼

(Get_GyroZero)
由於讀取HiTechnic Gyro回傳值僅需約3ms,因此程式範例中計算基準值的週期(ZERO_SAMPLE_PERIOD)即取4ms。而經執行程式後所得到之穩定的基準值(5004385)即可拿來作為計算角速度的參照,擷取的畫面如下:

(GyroZeroPoint)

計算角度位移
由於基準值在計算設定的過程中已經透過放大有效位數(64)以及加權(128)等兩個步驟,所以在計算角速度時,也一樣須先將所讀取的感應器回傳值乘以64128,以維持相同的精確度:
currVal = SensorRaw(GYRO_PORT) * SCALE * WEIGHT - gyroZero;至於角度位移(Angular displacement)的計算,就如同前面所說明的一樣,以取樣週期內的平均速度乘以期間長度,再經累加之後即可以得到:
gyroMove  += (currTick - sampleTick) * (currVal + lastVal) / 2;
此外在上面程式碼的計算式中,角速度的單位是:度/秒,而時間的單位是ms,所以當要還原成為以度(degree)來顯示位移時,須要將結果除以64*128*1000
NumOut (54, LCD_LINE4, gyroMove / (SCALE * WEIGHT * 1000));

陀螺儀(Gyro)與電子羅盤(Compass)本次測試另外以電子羅盤(Compass)作為對照,用來觀察陀螺儀所推算之角度位移與電子羅盤所回應之指向偏移間的差異,這樣就可以判斷本測試所使用的方式是否符合預期的功能,此外也可以使用NXT Servo Motor的角度感應器取代Compass作為對照。
而測試的機構如圖:
(HTGyroCompass-01)

而轉動的機構是以手搖方式以避免對電子羅盤的干擾,同時也較容易控制轉速以方便結果的觀察。


觀察測試過程

影片記錄共分成三個階段:基準值設定、由目前位置旋轉至-90度位置、再旋轉回原來位置,可以觀察到陀螺儀(Gyro)與電子羅盤(Compass)之間的差異(Diff(C-G))大致都維持在-3度以內,以結果而言,還算是符合預期的目標範圍:

(GyroCompassMove)

►NXC程式碼:
Compass作為比對:HTGyroCmps-Trvl.nxc
Compass比對功能:HTGyro-getTrvl.nxc






  • 留言者: Tim
  • Email: tg20317@yahoo.com.tw
  • 網址:
  • 日期: 2012-11-09 16:21:08
老師您好:
最近也有接觸Gyro, 想向您請問, 除了使用簡單平均做出一個基本值去消除掉零準位偏移量,
對Gyro自轉數圈, 量測到的角度發現不足360度, 轉了360度, 他卻只認得340度.

由於我使用的常數是對照DATASHEET上的數據(2.5mV/deg/s)再去積分,
請問Gyro除了零偏, 電壓角速度比值也會因為溫度而有偏差嗎?
如果有的話, 我該如何做出補償?
謝謝您的指教.
[版主回覆11/10/2012 17:05:00]
Gyro的測量值確實會受溫度影響而有偏差,
不過因為不是很清楚您是如何進行測量的, 若依據我的推測可能是累積誤差所造成,
也許可以試著在收集Gyro測量值時先不要換算成角度而直接使用電壓值積分,
等到要觀測時再轉換, 就可消除每次換算過程的誤差值
PS. 記得Zero Point也要先用電壓值






  • 留言者: 過客^_^
  • Email:
  • 網址:
  • 日期: 2011-11-07 19:18:21
老師你好:
雖然這文章有一段時間了,我還是來碰運氣問一下問題希望老師有看到。
我想請問一下,我大約知道加權平均法的應用,以及為何這樣使用,但是
我想請問一下這只針對基準值,但後來的傳回值並沒有做加權的動作只有提高
解析以及乘上128,這樣子傳回值不會因為陀螺儀的漂移產生誤差嗎??(傳回值 - 基準值)
因為我使用的是IDG500陀螺儀會有這樣的問題所以想請問老師更詳細的解說。

另外想請問老師的陀螺儀準確度到小數第幾位呢??
[版主回覆11/07/2011 23:06:54]一般商用的陀螺儀,尤其是導航用途的都會加上濾波的功能,用來平順化突波與漂移的現象, 建議可以用類似的方式, 控制讀取值的變化率若超過設定的門檻值, 就忽略掉。
這種控制方式算很常見, 尤其是應用在平衡機器人方面,應該可以從網路上找到不少資訊。







  • 留言者: Jimmy Hsu
  • Email: love750722@hotmail.com
  • 網址:
  • 日期: 2011-05-19 14:07:48
您好:

請教一下,WATCH_ZERO_PERIOD為何選擇256? 謝謝!

[版主回覆05/19/2011 21:30:45]WATCH_ZERO_PERIOD在Get_GyroZero()函式中是作為顯示結果的週期,
只要比讀取Gyro的取樣週期長就可以(4ms),  並沒有甚麼特別的考量





  • 留言者: yam
  • Email: dd901209@yahoo.com.tw
  • 網址:
  • 日期: 2011-03-15 11:00:09
請教老師

我目前用 VB2005 來寫陀螺儀轉速與時間的積分來取得角度

現在我已用VB 將 陀螺儀 的類比訊號轉換成轉速

再來就是時間的積分在VB 上要如何定義

[版主回覆03/15/2011 22:00:45]
如果已經能讀到陀螺儀的角速度值, 接下來可以參考本文的說明,
首先要先測得靜止狀態下陀螺儀的基準值,
作為計算運動狀態時角速度的offset值.
接下來, 因為無法知道角速度對時間的函數式,
所以才需要使用幾何的方式, 在極短的取樣週期內以平均速度 乘以 間隔時間
計算移動的角度, 再加以累計,
這樣就可以得到總角度位移的近似值.
這種計算方式也稱為黎曼積分(Riemann integral),
可以Google一下就查得到.
也因此, 這跟使用何種程式語言就無關,
至於我的範例程式(NXC), 就附在文章最後的連結中








  • 留言者: Flora
  • Email: yanhuasweet@hotmail.com
  • 網址:
  • 日期: 2011-02-15 00:01:28
請問一下,
計算零點值的時候
 zeroVal = zeroVal - (zeroVal/WEIGHT) + SensorRaw(GYRO_PORT) * SCALE;
因為有乘上SCALE(64),把數值放大了,所以顯示時要還原成原值.
可是還原回原值的時候(1.0000 * zeroVal / (SCALE * WEIGHT)),
為什麼是要除以(SCALE * WEIGHT),而不是除以SCALE(64)就好了呢?
謝謝!!

[版主回覆02/16/2011 22:33:04]
計算Gyro的基準值需要考量兩個因素:
1.
首先是因為使用整數運算,為消除捨位誤差所以在計算過程將讀取值乘上SCALE(64)
2.
其次,因為Gyro很容易受環境干擾而產生雜訊,亦即可能在計算過程讀取值偶而會有異常偏離(過高或過低)而影響結果,為克服此種現象所採用的方法就是“加權平均法”:
先設定一個權值(WEIGHT(128)),每次在累加讀取值前先減掉累計值的1/128後在加上讀取值,這樣偶發的雜訊對於整體累計值的影響就降低了,同時當讀取相當次數之後,累計值就會趨於穩定而不再受干擾,這時候的累計值就是基準值 * SCALE 128倍,這個可以用積分式去推算就可以瞭解。
舉一個例子作說明:
假設要量測開啟水龍頭流動中自來水的平均生菌數,我們可以準備一個可容納128單位的容器,並先裝滿無菌的蒸餾水,開始量測時先從容器中淆出一單位的水再倒入一單位的自來水,這樣每次倒入自來水的生菌數對容器平均生菌數的影響就降低到1/128而不致於受到偶發生菌數突然增加或減少的干擾,這樣的說明應該更清楚才是。







  • 留言者: simonapol
  • Email:
  • 網址:
  • 日期: 2010-12-06 15:45:32
那擷取時間假設25us一次很快的話我需要再將他累積加起來後在乘上回秒的單位嗎?

[版主回覆12/06/2010 21:42:40]應該不需要那麼複雜, 可以參考範例程式,
我是每32ms取樣一次, 再計算這段期間內的 平均速度 * 間隔時間 就可以得到移動的角度,
將所有取樣週期所計算得到的位移角度累加, 就可以得到總位移角度





  • 留言者: simonapol
  • Email:
  • 網址:
  • 日期: 2010-12-02 18:37:56
拜託老師講解

可否舉個例子看不出來計算過程搞不太懂弄混了
權重還有電壓變化量是什麼比例阿

數學可以交代清楚一點嗎?舉個實際例子例如角速度10度時或者傳回類比電壓1.5V時取樣下是怎麼計算出來的阿看不懂

不懂積分後是什麼值 看起來是指說我只要把現在的面積積分完就是角度/秒嗎?如果單位是秒

[版主回覆12/02/2010 22:43:09]讀取陀螺儀的值減掉靜止狀態下的基準值後就是角速度, 單位: 度/秒

因為, 陀螺儀很敏感, 為了要消除雜訊的干擾, 才會使用加權平均法的方式計算基準值,

知道角速度之後, 如果要測量一段期間內旋轉的 位移角度, 計算式 = 角速度 * 時間

所以, 角速度對時間的積分所得到的值就是位移的角度

所以, 結果不是 角速度 而是 角度 , 單位是 : 度