ジャイロセンサのドリフト

倒立振子ロボットを倒れないように制御する為には傾き角度を把握する必要がある。
傾きを関知するセンサとしてはジャイロセンサと加速度センサがあるのだが、ジャイロセンサは姿勢の変化を数値に表し、加速度センサは加速度の変化を数値に表すものなのだ。

ジャイロセンサは姿勢の変化(角速度)を積分し傾き角度を算出する。一方加速度センサは、地球上で常に鉛直方向に働いてる重力(加速度)を関知して傾きを算出することができる。
ただし水準器のような静止している状態(重力以外の力が加わってない無い状態)なら正確な傾き角度を算出できるのだが、倒立振子ロボットのような常に動いている物体には重力(加速度)以外の力(加速度)が働くので傾き角度を把握するのが難しいのだ。

傾き角度を求めるにはジャイロセンサの出番となるのだが、ジャイロにはドリフトというやっかいなものあるのだ。

LPC1114FN28でSTM製のジャイロL3GD20を接続して適当にジャイロセンサを傾けた記録がこれだ。

センサ単体の環境

センサ単体の環境

角速度

角速度

角度

角度

角速度は0度を中心に振れているが角度は徐々に増えている。ジャイロが壊れている訳ではないのだ。これがドリフトなのだ。

ドリフトの補正

ネットで倒立振子ロボットを格好良く立たせている方々は、何らかの方法でドリフトの補正をしているはずだ。
倒立振子ロボットの基礎を学んだIchiroさんはドリフト補正に最小二乗法予測を用いてドリフトを予想して補正してたのだが、最近カルマンフィルターという手法があるのを知った。

今回はカルマンフィルターを利用してみようかと思う。

カルマンフィルターの検証

大学で制御工学などを学んでたらカルマンフィルターはあれか!とピンと来るかも知れないが当方は無知なので良くは知らない。

幸いにもISHIDAさんのHPに”秋月 小型圧電振動ジャイロモジュール AE-GYRO-SMD/ENC-03R の使い方“という記事があり、Arduinoの環境で小型圧電振動ジャイロモジュール(AE-GYRO-SMD/ENC-03R)
のドリフトを3軸加速度センサモジュール KXR94-2050を用いたカルマンフィルターで抑える実験と検証をされている。

手持ちの死蔵部品庫には3軸加速度センサモジュール KXR94-2050はなく、ひとつ前の3軸加速度センサモジュール KXR52-1050ならあるので、これを用いて実験してみた。

カルマンフィルターの実験

カルマンフィルターの実験

カルマンフィルター適用したデータがこれだ。

カルマンフィルター適用のデータ

カルマンフィルター適用のデータ

ジャイロのみの場合は明らかにドリフトの影響を受けているが、カルマンフィルター適用した後のデータは収束している。

カルマンフィルターの評価

ISHIDAさんも書かれてるが、ENC-03Rは細工しても個体差が結構あり扱いが難しいセンサなのだが、加速度センサを利用しているとは云え使えるデータになっている。
ジャイロセンサと加速度センサ共に載っているSTM32F3-Discoveryもあるし単体の加速度センサも入手済みなので、カルマンフィルター適用して姿勢制御をしていこう。