カルマン・フィルタで ATOM Matrix 傾斜計 ーリアクションホイールへの道3ー

Home > 電子工作 > カルマン・フィルタで ATOM Matrix 傾斜計 ーリアクションホイールへの道3ー
すき 2
うんこ 0

これまではフライホイール付きブラシレスモータの制御について学習してまいりました。

ここでは1軸の姿勢制御モジュールの実現に向けて、カルマン・フィルタを用いて
M5Stack ATOM Matrix に搭載された6軸慣性センサ MPU6886 で姿勢角を導出します。
 

 

6軸センサ MPU6886

データシート
 https://m5stack.oss-cn-shenzhen.aliyuncs.com/resource/docs/datasheet/core/MPU-6886-000193%2Bv1.1_GHIC_en.pdf

 

センサのデフォルトの分解能設定は以下の通りです。

  • 加速度:16bit ±8 g
  • ジャイロ(角速度):16bit ±2000 deg/sec

https://github.com/m5stack/M5Atom/blob/master/src/utility/MPU6886.h

 

XYZ軸方向は以下の通りです。

 

M5Stack ATOM Matrix のY軸の傾き$θ$は加速度から以下で算出できます。

$$θ = \tan^{-1}\left(\frac{-accX}{accZ}\right)$$

 

カルマン・フィルタ

カルマン・フィルタについては2019年7月号のトラ技のコードを参考にしました。

 

以前M5StickCでもカルマン・フィルタによる傾斜計を製作していますので
詳細は以下を参照ください。

カルマン・フィルタで M5stickC 傾斜計 ー倒立振子への道 1ー

Arduinoコード

M5Atomライブラリはvar. 0.0.1を使用しました。
 https://github.com/m5stack/M5Atom

 

やってることの概要は以下の通りです。

  • センサのオフセット値を10回平均で導出し、以降測定値はオフセット引いて使用
  • センサばらつき算出:加速度とジャイロセンサを100回測って予め分散を導出
  • カルマン・フィルタの初期設定:使用する行列パラメータの初期値設定
  • タイマ割り込み(2.5msecごと)でカルマン・フィルタアルゴリズム発動させて傾斜角の推定値を算出
  • 50msecごとに加速度センサによる傾斜角とカルマン・フィルタによる角度の推定値をシリアル出力
  • M5AtomのLEDマトリクスに-30° ~ 30°の範囲でインジケータ表示

 

動作

青線:MPU6886の加速度センサから算出した傾斜角
赤線:カルマン・フィルタによる角度の推定値

加速度から導出した角度は外乱の影響を大きく受けていますが、
カルマン・フィルタによる角度は正確なセンサの姿勢角を示しています。

 

おわりに

カルマン・フィルタを用いてM5Stack ATOM Matrix の正確な姿勢角を得ることができましたので、
次回はモータと連動して1軸の姿勢制御モジュールの実現に向け進めていきたいと思います。

次の記事

ATOM Matrix の慣性センサでモータ制御 ーリアクションホイールへの道4ー

コメントはこちらから

メールアドレスが公開されることはありません。コメントのみでもOKです。

このサイトはスパムを低減するために Akismet を使っています。コメントデータの処理方法の詳細はこちらをご覧ください