加速度センサーとジャイロセンサーで

カルマンフィルターの実験(2/2)

前ページでは、３軸ジャイロセンサーの角速度と、角速度を角度に変換した値をＰＣに送り波形を
表示させて見ました。
なのでぇ、前ページを読んでない人は先に前ページを見る事をお勧めします。

《カルマンフィルター(Kalman Filter)》

ジャイロセンサーから得られるデータ値を、角度に変換した値(角速度を積分した角度値)は、
原点(０度)が時間と共にずれて行ってしまうドリフト現象が発生してこのままでは制御には扱いずらい値
となりました。

なら、角度で得られる３軸加速度センサーを使えばいいんじゃねぇと思うでしょう？
確かに加速度センサーが停止状態の時は、重力方向の角度がバッチリ得られます、だが、しか～あしぃ、
センサーが回転等の動作をしていれば重力以外の方向から加速(G)がセンサーに加わります、こうなると、
加速度センサーの値は狂った値となり正常に扱えません。

ジャイロセンサーは動いていても積分角度値は得られるがドリフトが有る、
加速度センサーはドリフトは無いですがぁ動いている時は使い物にならない．．．．困ったものです。

そこで、お互いのセンサーの良い所を合体させて値を取り出すのがカルマンフィルターです？。
カルマンフィルターについて私には難解ですので興味ある人はネットで検索して下さいませ。m(_ _)m

その他のドリフト対策は？
・角速度センサーはハイパスフィルタ、加速度センサーはローパスフィルタを付けて合成する方法
・相補フィルターを使う方法
などなど有るようです。

下図は、赤い線がジャイロセンサーX軸の積分角度値で、青い線が加速度センサーX軸の角度値を
ＰＣ(DataDisplay：前ページ参照)で波形表示させた画面で、ブレッドボードに２個配線し、同じ方向に
傾斜させた時の図です。
ジャイロセンサー(赤い線)はドリフトが発生していますね。
なのでぇ、ジャイロセンサーのみを利用する場合は、どこかでリセットする仕掛けが必要でしょう。

ジャイロセンサと加速度センサの波形図
尚、ジャイロセンサーと加速度センサーのX軸をカルマンフィルタに利用しますが、
図の様にジャイロセンサーX軸がマイナス方向の値なので反転させてデータの正負を揃える
必要が有ります。

次の下図が、カルマンフィルターを行った結果です、X軸のみでの計算＆実験です。
グラフを見るとぉ、すばらしぃ、ワンダフォー、バッチリですね。

カルマンフィルターの波形図
これを表示させるサンプルは”gyro-acceler.c”を動作させます。

カルマンフィルターはジャイロセンサーと加速度センサーの両方を使うのが特徴です。
但し、X/Y軸の2軸のみで、Z軸は加速度センサーが重力方向に傾けた時しか反応しないので
ジャイロセンサーの回転方向と合わないので計算不可です。

ＰＳ．
あぁ、ジャイロセンサーのZ軸は、方位センサーを使えばカルマンフィルター使えるかもぉ、
なのでぇ、こんな9軸センサーが有ったりするのかぁ．．．．

（配線について）

ジャイロセンサー(L3GD20：左)と加速度センサー(ADXL345：右)は、I2Cで接続します。
ジャイロセンサー(L3GD20)の話は、前ページを参照下さい。
加速度センサー(ADXL345)の話は、、”３軸加速度センサで傾斜角度を測定してみます”を参照下さい。

ジャイロセンサーのピン構成図

ＰＣに”超小型ＵＳＢシリアル変換モジュール”でデータを送信する為にUSARTを使いますが、
モジュール端子の配列は左から[5V][GND][TX][RX]です、PICからはTX1(RC6)の送信側のみ配線します
[5V]はUSBから出力される電源ですので配線はしないで下さい。(下配線図参照)

カルマンフィルターの配線図
このブレッドボードの右左側横方向が各センサーのＸ軸方向となります、
なのでセンサーの取り付け方向は注意して下さい。

ＬＣＤはお好みで取り付けて下さい、サンプルプログラムではＬＣＤに表示させています。
(まぁ、センサーを傾けた場合の値を見ておいた方が良いでしょう)

（サンプルプログラムについて）

今回のPIC18F26K22のCPUクロックは６４ＭＨｚで動作させています。
前ページの様にＰＣにUSARTでデータを送るだけのプログラムとなります。

↓ここからサンプルプログラムソースファイルをダウンロードして下さい。
KalmanFilter．lzh

プログラムソースをダウンロードしたら、MPLAB X(v2.15)にてプロジェクトを作成します。
以下のファイルをプロジェクトディレクトリにコピーしてプロジェクトに取込んで下さい。
次にコンパイルとＰＩＣ書き込みを実行して下さい。
MPLAB(R) XC8 C Compiler Version 1.32コンパイラを使用しています。

ダウンロードファイルを解凍すると下記の様なファイル構成です。
　gyro-acceler.c･･･････本体のサンプルソースプログラム(カルマンフィルタ表示用)
　skADXL345I2C.c･････ADXL345関数ライブラリソースファイル(I2Cバージョン)
　skADXL345I2C.h･････ADXL345ライブラリ用ヘッダファイル(I2Cバージョン)
　kalman.c･････････････カルマンフィルタ関数ライブラリソースファイル
　kalman.h･････････････カルマンフィルタイブラリ用ヘッダファイル
　gpl2.txt･･････････････GPL2のライセンス詳細テキストファイル

　※ こちらの"skADXL345I2C"ライブラリは使えません、ここのページのを使います。

　上記のライブラリ以外に下記ライブラリも必要ですが、前ページの物を使います。

　skL3GD20I2C.c･････L3GD20関数ライブラリソースファイル(I2Cバージョン)
　skL3GD20I2C.h･････L3GD20ライブラリ用ヘッダファイル(I2Cバージョン)
　skI2CLCDlib.c･･････Ｉ２Ｃ接続ＬＣＤ関数ライブラリソースファイル
　skI2CLCDlib.h･･････Ｉ２Ｃ接続ＬＣＤライブラリ用ヘッダファイル
　skI2Clib.c･･････････Ｉ２Ｃ通信を行う関数ソースファイル
　skI2Clib.h･･････････Ｉ２Ｃ通信を行う関数のヘッダファイル
　skUARTlib.c････････ＵＳＡＲＴ通信を行う関数ソースファイル
　skUARTlib.h････････ＵＳＡＲＴ通信を行う関数のヘッダファイル
　millis.c･････････････プログラム起動時からの時間カウントを行う関数ライブラリソースファイル
　millis.h･････････････プログラム起動時からの時間カウントを行う関数用ヘッダファイル

カルマンフィルターライブラリの事

“kalman.c””kalman.h”はこちらのサイトの物です、
このサイトは、センサーにMPU6050(ジャイロ・加速度両用)を使用し、Arduinoでプログラミングされた
物で有りこれをPIC用に改編しました。
PIC用に改編した物は、１軸のみ計算できるライブラリで有り、２軸で使用するには改造が必要です。
尚、改編前のライブラリはライセンスが”GPL2”なので、”gpl2.txt”ファイルも付けて置きました。

改編前のライブラリはMPU6050で調整されていると思うのですがぁ、
例えば、“kalman.c”に以下の記述が有ります。

    Q_Angle   = 0.001f;  // 加速度のシステムノイズの共分散行列パラメータ値
    Q_Bias    = 0.003f;  // ジャイロバイアスのシステムノイズの共分散行列パラメータ値
    R_Measure = 0.03f;   // 観測ノイズの共分散行列パラメータ値

この値が調整値ですが、ちょいとぉ意味不明なので取りあえずこのまま使用です。
(データに含まれるノイズの含有成分により調整すると思うがぁ．．．．)

又、カルマンフィルターライブラリの使い方は、"gyro-acceler.c"を見ましょう。

ｓｋＡＤＸＬ３４５Ｉ２Ｃ．ｃ

このライブラリはADXL345を使った3軸加速度センサモジュールにアクセスする為の関数集です。

この関数集はI2C接続なので、skI2Clib．c / skI2Clib．hのファイルが必要です。
この関数集を利用する場合は、"skI2Clib．c"の"InitI2C_Master( )"関数を呼び出した後使います、
使い方の例は、"gyro-acceler.c"を参照下さい。

ADXL345にアクセスを行う関数の使い方を説明します。

　ans = acceler_Init( )
　　ADXL345の初期化を行う処理です。
　　分解能は10bitモードで出力、使用するレンジは±２Ｇで出力データレートは200Hz、
　　またFIFOは使用しないで初期化しています。
　　　ans : -1ならADXL345が応答していない、正常ならADXL345のID(0xE5)が返ります。

　ans = acceler_Read(*X,*Y,*Z)
　　ADXL345の各軸の値を読み込む処理です。
　　XYZ軸のセンサ値を読み込んで指定の変数にデータを返します、大体±300内位の値です。
　　　*X　: X軸の値を読み込みこの変数にデータを設定
　　　*Y　: Y軸の値を読み込みこの変数にデータを設定
　　　*Z　: Z軸の値を読み込みこの変数にデータを設定
　　　ans : ０＝正常　－１＝読込み失敗(I2C異常)

　　例）
    int X , Y , Z , ans ;

    ans = acceler_Read(&X,&Y,&Z) ;
    if (ans == 0) {
         // 各軸の表示を行うなどの処理を行う
    }

　ans = acceler_AVEread(su,*X,*Y,*Z)
　　ADXL345の各軸の値を指定の回数(su)だけ読込み平均化したデータを返す処理です。
　　XYZ軸のセンサ値を読み込んで指定の変数にデータを返します。
　　　su　：何回のデータを読み込んで平均化するかの個数を指定
　　　*X　： X軸の値を読み込みこの変数にデータを返す
　　　*Y　： Y軸の値を読み込みこの変数にデータを返す
　　　*Z　： Z軸の値を読み込みこの変数にデータを返す
　　　ans ：０＝正常　－１＝読込み失敗(I2C異常)

《その他》

そのうちに、9軸センサーを手に入れてカルマンフィルターの実験を行い、
カルマンフィルターのライブラリも３軸対応にしたいなぁっとぉ。

［前ページ"３軸ジャイロセンサー出力を波形で表示する記事"へ］

加速度センサーとジャイロセンサーで カルマンフィルターの実験(2/2)

《カルマンフィルター(Kalman Filter)》

（配線について）

（サンプルプログラムについて）

《その他》

加速度センサーとジャイロセンサーで

カルマンフィルターの実験(2/2)