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

Home > 電子工作 > カルマン・フィルタで M5stickC 傾斜計 ー倒立振子への道 1ー
すき 0
うんこ 0

今月のトランジスタ技術が非常に興味深い内容でした。

 

確率統計コンピューティング特集ということでカルマン・フィルタの歴史とカルマン倒立振子の作り方を非常に丁寧に紹介しています。

必要な数学や力学の基礎も記載されておりロボット制御の入門として最適なのではと思います。DVDでのセミナーも易しくありがたい限りです。

はっきり言ってお父ちゃんはカルマン・フィルタとは何ぞやというレベルで全く知識のない状況です。そこで倒立振子への道と題してM5StickCを用いた倒立振子の実現を目指して一歩ずつお勉強していこうと思います。

ここではトラ技のコードを参考にカルマン・フィルタを用いた傾斜計を作ってみたいと思います。カルマン・フィルタの素晴らしさをまずは体験してみようと思います。

 

 

M5stickC

M5stickCには6軸の慣性センサ(加速度、ジャイロ) SH200Q が内蔵されていますので、このセンサ値を用いてM5stickC自身の傾きを算出してみます。

6軸センサ SH200Q

SH200Q データシート
 https://github.com/m5stack/M5-Schematic/blob/master/Core/SH200Q.pdf

SH200QのArduino IDEサンプルコードは以下にございます。
 https://github.com/m5stack/M5StickC/blob/master/examples/Basics/SH200I/SH200I.ino 

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

参考:144Labグループ開発者ブログ

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

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

 

上の写真のようにM5stickCを置いてSH200Q のサンプルコードを動かすとSH200Qから出力されるデータは以下のようになるはずです。

  • 加速度:AccX = 0,  AccY = 0, AccZ = 4096(+1g)
  • 角速度 : GyroX = 0, GyroY = 0, GyroZ = 0

しかしセンサのオフセットのため実際に出力される値にはずれがありましたので、今回はあらかじめオフセットを測って引いて使用します。

カルマン・フィルタを用いた傾斜計に関してはセンサのデフォルトオフセットの影響はないかもしれませんが今回は生データとの比較も行いたいのでオフセット引いております。

カルマン・フィルタによる傾斜導出の概要

ここではカルマン・フィルタの詳細理解はすっ飛ばして とにかく使ってみてご利益を実感するのを目的としています。ですので詳細はトラ技や他に譲るとしてざっくりとした概要です。

カルマン・フィルタは測定値と数理モデルによる予測値をうまいこと利用してもっともらしい傾斜角を導出します。

加速度センサによる角度

重力加速度のY軸、Z軸の成分よりM5stickCの傾斜角θは以下の式で求められます。

$$θ = tan^{-1}\left(  \dfrac{g_z}{-g_y} \right)$$

しかし加速度センサは振動などの重力加速度以外の加速度も検出してしまう問題があります。

ジャイロセンサによる角度

ジャイロセンサのx軸の角速度$ω_x$を時間積分することでM5stickCの角度を求めることができます。

しかしジャイロは初期姿勢の角度は検出できません。さらにセンサ測定値にはオフセット誤差が含まれるので誤差も積分されて増幅していきます。

モデルによる予測

上記のそれぞれ短所をもった測定値とそのバラつき誤差を加味した数理モデルをつくって傾斜角度の予測値を算出します。

測定バラつきとして予めセンサを複数回測定(ここでは100回)して分散を求めます。

モデルの詳細はまだ理解できてないので割愛します。いつか理解できるその日まで。。。

カルマン・フィルタのアルゴリズム

上記の測定値とその分散そして数理モデルによる予測値をうまいこと用いて角度の推定値を得ます。

内容は全く分からないので後に回します。トラ技紙面に譲ります。。今は分からない。。。今は。。。

傾斜計コード

Arduino IDEをもちいでカルマン・フィルタM5stickC 傾斜計を作製しました。比較のために加速度センサによる傾斜角とカルマン・フィルタによる推定値を出力します。

トラ技のソースコードInclinometer.cppをほぼ丸々参考にしています(p. 49 – 52)。

 

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

  • センサばらつき算出:加速度とジャイロセンサを100回測って予め分散を導出
  • カルマン・フィルタの初期設定:使用する行列パラメータの初期値設定
  • タイマ割り込み(2.5msecごと)でカルマン・フィルタアルゴリズム発動させて傾斜角の推定値を算出
  • 50msecごとに加速度センサによる傾斜角とカルマン・フィルタによる角度の推定値をシリアル出力

カルマン・フィルタアルゴリズムについての詳細はしつこいですがトラ技紙面か以下の動画を参照ください!

 

 

動作

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

 

最初の感想は”凄い!”でした。振ると当然 加速度の外乱が入るので、加速度センサによる角度(青線)は乱れますがカルマン・フィルタによる角度の推定値(赤線)に大きな乱れはありません。

しかも加算平均とかではなくあくまでリアルタイムのセンサ値と前回算出した予測値で算出されているようなのです。

なんかすごいね。。理解できないけど。

 

今回スタートした 倒立振子への道 。どうなるものか。たどりつけるのか。

全くの未知ですが 今回カルマン・フィルタのご利益を全身で堪能出来て興奮気味なので頑張りたいです!!

次の記事

傾斜計 カルマン・フィルタ アルゴリズムの解きほぐし ー倒立振子への道 2ー

「カルマン・フィルタで M5stickC 傾斜計 ー倒立振子への道 1ー」への4件のフィードバック

  1. 初心者なのでソースを見てもなかなか理解できませんがいつも興味深く拝見しています。
    ところで、公開されている上記をコンパイルすると以下のエラーが出ます。
    no matching function for call to ‘IMU::getAccelData(int16_t*, int16_t*, int16_t*)’
    int16_tとfloatの違いもわかっていませんが、int16_tだとgetAccelADCになると思いますが。
    ご教示頂けると助かります。

    1. コメントありがとうございます。
      私のM5stickCは6軸センサがSH200Qの初期のもので記事のコードも古いライブラリで動作させております。

      お使いのM5stickCの6軸センサの型式をご確認いただいて、適宜floatに変更かgetAccelADCを使用かご検討ください。

      宜しくお願い致します。

      1. 返信ありがとうございます。
        搭載デバイスがいくつかあってそれぞれ対応するコードが違うんですね。中身わからないままですが色々試してみます。

        1. M5StickCの裏のシールに各部品の型番が記載されていますのでご確認よろしくお願い致します。

          進捗楽しみにしております。

コメントはこちらから

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

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