【Arduino】RCフィルタをプログラムで作ってみた.

ドローンを作る上で必要だった,ノイズフィルタ.

モーターの乗っているドローンの9軸センサからくるデータっていうのは,ノイズが多く,単純な積分だとすぐ変な角度を出力するようになりました.参照:状態推定(姿勢角)

そこで,一番最初に使ってみたRCフィルタについて書いていきたいと思います.結果的にはドローンには使いませんでした.


フィルタをプログラムで使えるようにするには,差分方程式というものを使います.

プログラムは,連続的なアナログ回路のように計算することができません.差分方程式とは,「差分」というように,飛び飛びの値を使用することのできる方程式です.


まず最初に,ドローンで使う上で,角度が欲しかったので,そのために,ジャイロ入力を積分する必要があります.


最初は単純な台形積分をしました.

台形積分の伝達関数は「1/s」として近似できるはずです(多分).これを使用したのですが,動かしてみるとすぐに値がズレました(ドリフト).


ということで,次に使用したのは,RCフィルタです.

RC積分と呼ぶかもしれません. ドローンの動きの周波数はせいぜい数十[Hz]なので,それ以上の周波数はいらないものとしてカットしちゃおうという試みです.

RCとは,R:抵抗,C:コンデンサを組み合わせたローパスフィルタ(LPF)で,伝達関数は「1/(Ts+1)」または 「Rc/(Rc+Ts)」です. すなわち,一次遅れ系です.

ローパスフィルタは実は積分器なんですね.
だから,ジャイロセンサの角速度を入力すると,角度が出てきます

計算式と差分方程式は以下です(多分). 「1/s」から「1/(Ts+1)」になりました.

そして式は,y[n] = (1-a) * x[n] + a * y[n-1]    です.

y[n]:角度
x[n]:ジャイロの出力値
y[n-1]:前回の角度の出力値
a      :フィルタの定数

aについて: a = RC / (RC + Ts) RC=Tとして,Rcをカットしたい周波数の周期[s]とし,Tsをプログラムのループ時間[s]とします. (例)プログラムのループ時間0.02[s]で,カットしたい周波数が10[Hz] = 周期0.1[s]の場合は, a = (0.1 / (0.1 + 0.02) = 0.833  となります.

下の図のように,RCフィルタを使用すると値がずれても,その後,値は0に収束していきます.(とても大雑把に書きました) あとは,必要な周波数に合わせて,aの値を変えていくだけです.


このフィルタは,いい感じにドローンに適応させることができなかったので,使用しませんでしたが,他のものを作る時に使う機会がありそうです.

アナログのRCフィルタが,プログラムでも同じように動作するということに,意外とすごいなと思いました.

間違いなどあるかもしれないので,自分で確認してから使ってください!
間違いがあったら,ぜひコメント欄に指摘お願いします.

5件のコメント

  1. お!
    ノイズフィルター!

    …と思って読んだのですが、悲しいくらい自分に理解能力が無いことがわかりました…。
    そもそも自分は、ノイズフィルターというのは、抵抗とコンデンサでできるものだと思っていました。それが…こんなに複雑な計算が必要だなんて…思ってもいませんでした。
    くじけそうです(笑)。

    それはそうと、本文とは関係ない事をお聞きして申し訳ないのですが、現在サーボモーターが届くまでの間、Arduinoとアプリを繋いで、Arduino側の数値の受け渡しとアプリへの表示というのを、Bluetoothを介して行おうと思っているのですが、Android studioなるものが重くて自分のPCでは使えません。

    そこで、別の方法を試したいと思っているのですが、何か良い方法をご存知だったら、ご教授いただけませんでしょうか。Ecripseと言うものもあったのですが、自分には敷居が高くて…。
    Monacaという、アプリを簡単に作れるものもあったのですが、これでArduinoを接続する方法が分からず、頓挫しています。

    何か知識をお持ちでしたら、ヒントをお与えくださいませ。
    よろしくお願いいたします。

    使っているBluetoothはRN-42と言うモデルです。

  2. すみません。書き込みを読み返してみて、何でも人に尋ねてしまうのは良くない癖だと思いました。
    情報共有できる仲間もいないものですから、甘えてしまいました。
    周りにもう少し色々と自分で試行錯誤をしてみようと思います。

    いつもありがとうございます。
    記事の更新をいつも楽しみにしています。

    1. 自分もすべて独学でインターネットを調べてやってきました.
      大体のことはインターネットに乗っていると思います.

      ちなみに,ノイズフィルターの記事は,ECUなどには使わないですよ.
      加速度センサーやジャイロセンサーなどに使えるものです.

      抵抗やコンデンサなどをフィルタで大丈夫です.

      1. ご助言、ありがとうございます!

        とりあえずインターネットで調べて、どうしても分からなかったら質問させてください。

        よろしくお願いします。
        いつもありがとうございます。

返信を残す

メールアドレスが公開されることはありません。