電子工作‎ > ‎

姿勢制御

加速度センサとジャイロセンサを使った姿勢検出についてのページです。

おもに傾き検出について書いています。
 

使用センサ


回路図

I2Cでモジュールと通信できます。
回路図を見ればわかりますが、SDAとSCLは既にプルアップされているので外部にプルアップ抵抗は必要ありません。


単純な傾き検出


・加速度センサを使った傾き検出

加速度センサはセンサにかかる加速度を検出します。
地球上にいる限りは、全ての物体に重力がかかっています。重力も物体を加速させようとする力なので加速度です。

センサが水平な状態だとすれば、加速度はZ軸方向のみに1[g]≒9.8[m/s^2]だけかかります。
X軸を中心にセンサが回転すれば、Y軸方向にも加速度がかかり始めます。
また、Y軸を中心にセンサが回転すれば、X軸方向に加速度がかかり始めます。

つまり、重力の大きさと、X軸、Y軸方向の加速度が分かっていれば、三角関数より角度が求められます。

 - X軸中心の回転検出
重力の大きさは

 √(z^2 + y^2)

で求められます。
傾きの角度は 

 θx = asin(y / √(z^2 + y^2)) 

です。

- Y軸中心の回転検出
重力の大きさは

 √(z^2 + x^2)

で求められます。
傾きの角度は

 θy = asin(x / √(z^2 + x^2))

です。

*演算上の注意
加速度センサから送られてくるデータは16bitです。
演算では2乗和を取っているため、32bit整数型ではオーバーフローする可能性があります。(0xFFFF * 0xFFFF = 0x1FFFC0002)
long型か、float、double型を使うといいと思います。
(ここらへんの詳しい話は省略)


・ジャイロセンサを使った傾き検出
ジャイロセンサはセンサにかかっている角速度を出力します。
速度を積分すれば位置になるように、角速度を積分すれば角度になります。

コンピュータで行う積分の一番簡単な方法は、センサの値をどんどん足していくことです。
実際には、サンプリング周期(センサからデータをゲットする間隔)を考慮してやる必要があります。
tを時間、d(t)を時間tのときのデータ値、Δtをサンプリング周期として式にすると

y = ∫d(t)dt ≒ Σ (d(t) * Δt)

と、表せます。
プログラム的に書くと

out = 0
samplingtime = 0.1

while 1:
    data = getdata()
    out = out + data * samplingtime

こんな感じです。

ここで注意ですが、センサから得る値を、[rad/s]単位に変換する必要があります。
でないと積分したときに角度が[rad]の値になりません。

[deg/s]でもかまいませんが、C言語などの数学ライブラリは基本的に弧度法を用いているので、弧度法のほうがおススメです。

センサから出力される値は、レンジによって異なります。
データシートを見ると分かりますが、「ジャイロセンサのフルスケール出力範囲(the full scale range of the gyroscope outputs)」などの項目に±200[°/s]のようなことがかいてあると思います。
フルスケールレンジが200[°/s]で、データが16bitだとすると、出力1bitあたりの角度は
200 / 65535 ≒ 0.003[°/s] ≒ 5.3264e-5[rad/s]
となります。

このことに注意して、それぞれ調整を行ってください。


この処理を、X軸、Y軸それぞれ行います。


ちょっとだけマトモな傾き検出

加速度センサとジャイロセンサ、それぞれで傾きを検出する方法について書きました。
しかし、加速度センサにもジャイロセンサにも、それぞれ欠点があります。

・加速度センサの欠点
加速度センサで傾きを求める際、重力加速度を元に計算していました。
なので、重力加速度以外の加速度が加わってしまうと、前提が崩れてしまうため正しく計算できません。

・ジャイロセンサの欠点
ジャイロセンサでは、回転していない時の出力、ゼロ点を元に角速度を求めます。
しかし、ジャイロセンサにはゼロ点がだんだん変化していく特性(ドリフト)があります。周囲の温度によってドリフトしたり、単なる時間変化でドリフトしたりします。
ジャイロセンサは角速度値を積分して角度を求めていました。ドリフトが起こってしまうと、静止しているはずなのに角度が変化していっている、というようなことが起こってしまいます。

加速度センサでは、加速度が加わってしまうと正しい角度が検出できなくなるため、基本的には角度検出にはジャイロセンサを用います。
しかし、ジャイロセンサにはオフセット誤差やドリフトがあるため、誤差の蓄積が起こってしまうことになります。

例えば倒立振子などの制御で誤差の蓄積が起こってしまうと、段々誤差が大きくなり、制御不能に陥ると思います。

なので、オフセットやドリフトを打ち消すような処理が必要になります。



・LPFとHPFを用いる方法
加速度センサ

LPF(ローパスフィルタ)とHPF(ハイパスフィルタ)を加速度センサとジャイロセンサの出力にかけて、オフセットやドリフトを打ち消します。
LPF,HPFが何者かわからない人は調べてください。

式に表わすと、

出力 = LPF(加速度センサーの値) + HPF(ジャイロセンサーの値)

です。
だいたいこんな感じになります。


赤:加速度センサ
緑:ジャイロセンサー
青:LPFとHPFをかけた出力

ジャイロセンサーの出力がだんだんドリフトしてしまっているのがわかります。
また、加速度センサーも、水平に力を加えるなどをしてしまうと値がおかしくなってしまいます。

LPFとHPFをかけると、ジャイロセンサーのドリフトが打ち消されたような出力になっています。



・相補フィルタを使う方法
LPFとHPFを使う方法を簡略化したもの(?)です。

↓参考資料

Output = 0.95 * (Output_before + gyro) + 0.05 * accelerate
(Output:出力 Output_before:1回前のOutput gyro:ジャイロセンサで得た角度 accelerate:加速度センサで得た角度)


・カルマンフィルタを使う方法



・パーティクルフィルタを使う方法



・ティーポッドを回す

Comments