加速度センサーとジャイロセンサーで
カルマンフィルターの実験(1/2)
〔２／２〕
〔マイコンのトップに戻る〕
カルマンフィルター(Kalman Filter)については次ページ(2/2)で記述します。
今回のセンサーは、３軸加速度センサーADXL345と３軸ジャイロセンサーL3GD20を使います。
両方ともI2CかSPIで接続しますが、今回はI2Cで行います。
３軸加速度センサーADXL345の事は、”３軸加速度センサで傾斜角度を測定してみます”を参照
して下さい。
このページでまずは、３軸ジャイロセンサーL3GD20からデータを読み出して見ます。
PS. *1)
2020/03現在"3軸ジャイロセンサーL3GD20"モジュールは秋月通商では販売されていません、
ストロベリー・リナックスのこちらのモジュールをつかいましょう。
《３軸ジャイロセンサー》
下の図は、ジャイロセンサー(左)と加速度センサー(右)の各軸における動作方向の概念図です。
ジャイロセンサーは、各軸の回転方向の加速度が得られます、なのでセンサーが停止している時と
定速度の場合のセンサー値は０dpsとなり、動作している時のみ角速度(角加速度)が得られます。
なのでぇ、何方かの方向にどの位の速さ(加速)で傾き中(回転中)と言う事が解ります。
加速度センサーの各軸は、重力方向の傾斜角度が得られます、
なのでぇ、水平状態であればＸＹ軸は０度で、Ｚ軸は重力方向に90度(1G)の値が得られるはずです。
ジャイロセンサー 加速度センサー
L3GD20のこと
測定範囲 ： ±250dps ±500dps ±2000dps (今回は±250dps/0.00875dps)
分解能 ： 0.00875dps 0.0175dps 0.07dps
データ更新のレート ： 95Hz 190Hz 380Hz 760Hz (今回は760Hz)
電源電圧 ： 2.4V〜3.6V
インターフェイス ： I2C/SPI (今回はI2C)
・温度センサー(8ビット／-40℃〜85℃)も付いています。(今回未実験)
・各軸の出力は16ビットです。
・32スロットのFIFO機能有り。(今回未実験)
レジスタの構成等の説明は省きます、
データシート等はストロベリー・リナックスのモジュール販売ページを見ましょう。 *1)
PICからUSARTで送られてきたデータを下図の様に波形を表示させるパソコンのソフトです。
このソフトはシリアル通信を通じてデータを送ればグラフを描きます。
なので、マイコンのＡＤＣでデータを読んでシリアルで送れば波形を表示出来ると言う物で、
３チャンネルまで対応しています。
こちらのVecterからダウンロードして下さい、インストールは不要です、
解凍したら"DataDisplay.exe"を実行するだけです。
古いソフトらしく動作環境が、”Windows XP/Me/2000/98”で、私のWindows7は動作しました。
起動させたら「ポートの設定画面」が出るので、”COMポート”と”ボーレート(19200bps)”を設定します。
グラフ画面が表示されたら、画面の右上に「データビット長：」と有りますね、
これを"10Bit"に設定します。
DataDisplay.exeの有るフォルダーに"DATALOG"フォルダーが有ります、これは表示したデータを
自動的に保存しています、なのですがこれを削除する機能がないので不要なら定期的に手動で削除
しないとファイルは溜まるだけですので注意が必要です。
下の図は、加速度センサーの各軸をＰＣに送り、それぞれの方向に上下左右動かしてみた画面です、
Ｚ軸は既に１Ｇの加速度がかかっていますね。(センサー値そのままの値です)
下の図は、ジャイロセンサーの各軸の角速度(dps)を表示させています。
(センサー値から角速度に変換した値です)
角速度は、出力値に各レンジの分解能の値を掛けます。(±250dpsなら0.00875dpsを掛けます)
これを表示させるサンプルは”gyro21.c”を動作させます。
この角速度で姿勢制御させるはちょっとぉ難しいでしょう、なので角速度を角度に変換します。
変換には、台形法により一定時間ごとに定期的に読込み周期毎の移動量を累積加算すると
全体の移動角度となるらしぃ。
詳しい内容は、ネットで”ジャイロセンサー 台形法”などで調べて下さい。
下の図が、角速度を積分により角度計算させて表示させた結果です。
(Ｚ軸のみ左右に回転させています)
これを表示させるサンプルは”gyro22.c”を動作させます。
角度は出せたのですがぁ、原点が時間と共にずれて行っています(ドリフト現象)。
この弱点を克服する為にカルマンフィルターを使う事にします。
ドリフトとは
センサーが静止しているのに出力が出ると言う誤差が生まれます、
これは温度変化や静電容量に振動等のノイズで発生するらしい、
これを積算して行っているのでどんどんずれるのでしょう？
（配線について）
左図は、18F26K22のピン構成です。
回路電源はセンサが3.3Vなので3.3Vです。
MSSPは２回路有りますが、今回のI2C通信は、
SDA2/SCL2を利用して、センサとLCDに接続します。
I2C用のプルアップ抵抗は、PIC内部のプルアップを
使います。
LCDのプルアップ用の半田盛りは行いません。
ＰＣに”超小型ＵＳＢシリアル変換モジュール”でデータを送信する為にUSARTを使いますが、
モジュール端子の配列は左から[5V][GND][TX][RX]です、PICからはTX1(RC6)の送信側のみ配線します
[5V]はUSBから出力される電源ですので配線はしないで下さい。(下配線図参照)
左図は、ジャイロセンサーのピン構成です。
センサーは、SPIかI2Cで接続できますが今回はI2Cです。
CS ：I2C接続時はVDDに配線
SDO/SA0：I2C接続時はこのピンでアドレスを選択。
VDDに接続にするとアドレスは0x6Bで、
GNDに接続にすると0x6Aです。
(今回はGND接続で0x6Aに設定)
INT1/INT2：イベント発生時に通知する為の割り込み出力ピン(今回は未使用)
PS, *1)
ストロベリー・リナックスのモジュールの場合は、上図のピン1-2間にVDD_IOピンがあります、
VDDに配線すれば良いでしょう。
又、上図のピン8-7間はNCピンで2ピン増えています、後は同じだと思いますが動作させていないので何ともです。
実態配線図です、センサーは右上が１番ピンです。
上のＬＣＤはこちらの物を使っています、ＬＣＤは無くてもかまいませんが、
実際にどんな値が出力されているか確認した方が良いでしょう。
尚、ＬＣＤについては、”秋月電子Ｉ２Ｃ接続小型ＬＣＤモジュールに表示を行う”を参照下さい。
今回のPIC18F26K22のCPUクロックは６４ＭＨｚで動作させています。
↓ここからサンプルプログラムソースファイルをダウンロードして下さい。
L3GD20．lzh
プログラムソースをダウンロードしたら、MPLAB X(v2.15)にてプロジェクトを作成します。
以下のファイルをプロジェクトディレクトリにコピーしてプロジェクトに取込んで下さい。
次にコンパイルとＰＩＣ書き込みを実行して下さい。
MPLAB(R) XC8 C Compiler Version 1.32コンパイラを使用しています。
ダウンロードファイルを解凍すると下記の様なファイル構成です。
gyro21.c･･･････････本体のサンプルソースプログラム21(角速度表示用)
gyro22c････････････本体のサンプルソースプログラム22(積分角度表示用)
skL3GD20I2C.c･････L3GD20関数ライブラリソースファイル(I2Cバージョン)
skL3GD20I2C.h･････L3GD20ライブラリ用ヘッダファイル(I2Cバージョン)
skI2CLCDlib.c･･････Ｉ２Ｃ接続ＬＣＤ関数ライブラリソースファイル
skI2CLCDlib.h･･････Ｉ２Ｃ接続ＬＣＤライブラリ用ヘッダファイル
skI2Clib.c･･････････Ｉ２Ｃ通信を行う関数ソースファイル
skI2Clib.h･･････････Ｉ２Ｃ通信を行う関数のヘッダファイル
skUARTlib.c････････ＵＳＡＲＴ通信を行う関数ソースファイル
skUARTlib.h････････ＵＳＡＲＴ通信を行う関数のヘッダファイル
millis.c･････････････プログラム起動時からの時間カウントを行う関数ライブラリソースファイル
millis.h･････････････プログラム起動時からの時間カウントを行う関数用ヘッダファイル
ＤａｔａＤｉｓｐｌａｙ用関数について
この関数は本体のサンプルソースプログラム(gyro21.c/gyro22.c)に記述されています。
ans = DataDisplay(kosu, d1, d2, d3)
センサーから読込んだデータを"DataDisplay"用のデータに変換する処理
変換データのフォーマットは、ダウンロードし解凍した「DataDisplayの使用法．pdf」を
見て下さい。
kosu ：３チャンネルの内で、何チャンネル分のデータを送るか設定(1-3)します
d1 ：１チャンネル用のデータ(X)を指定します
d2 ：２チャンネル用のデータ(Y)を指定します
d3 ：３チャンネル用のデータ(Z)を指定します
ans ：変換したデータの格納先アドレスを返します
例）１チャンネル分のデータを送る場合
int x ;
x = analogRead(3) ;
Serial.print(ADCdisplay(1,x,0,0)) ; // X Ch:REDでグラフ表示される
ｓｋＬ３ＧＤ２０Ｉ２Ｃ．ｃ
このライブラリはL3GD20を使った3軸ジャイロセンサモジュールにアクセスする為の関数集です。
この関数集はI2C接続なので、skI2Clib．c / skI2Clib．hのファイルが必要です。
この関数集を利用する場合は、"skI2Clib．c"の"InitI2C_Master( )"関数を呼び出した後使います、
使い方の例は、"gyro21.c"を参照下さい。
L3GD20にアクセスを行う関数の使い方を説明します。
ans = gyro_Init( )
L3GD20の初期化を行う処理です。
まず、"WHO_AN_I"のアドレスを読みIDが正常に得られれば初期化処理を行います。
データレートは760Hzで、測定レンジは±250に設定で初期化を行っています。
ans ： -1ならL3GD20が応答していない、正常なら０が返ります。
ans = gyro_Read(*X,*Y,*Z)
L3GD20の各軸の値を読み込む処理です。
XYZ軸のセンサ値を読み込んで指定の変数にデータ(センサー値そのままのデータ)を返します。
*X ： X軸の値を読み込みこの変数にデータを返す
*Y ： Y軸の値を読み込みこの変数にデータを返す
*Z ： Z軸の値を読み込みこの変数にデータを返す
ans ： ０＝正常 −１＝読込み失敗(I2C異常)
ans = gyro_AVEread(su,*X,*Y,*Z)
L3GD20の各軸の値を指定の回数(su)だけ読込み平均化し、角速度に変換した値を返す処理です。
XYZ軸のセンサ値を読み込んで指定の変数にデータを返します。
su ： 何回のデータを読み込んで単純平均化するのか個数を指定
*X ： X軸の値を読み込みこの変数にデータを返す
*Y ： Y軸の値を読み込みこの変数にデータを返す
*Z ： Z軸の値を読み込みこの変数にデータを返す
ans ： ０＝正常 −１＝読込み失敗(I2C異常)
ｓｋＩ２ＣＬＣＤｌｉｂ．ｈ／ｓｋＩ２ＣＬＣＤｌｉｂ．ｃ
Ｉ２Ｃ接続ＬＣＤにアクセス(表示)を行う為の関数集群です。
この内容はこちらの「秋月電子Ｉ２Ｃ接続小型ＬＣＤモジュールに表示を行う」を参照下さい。
但し、
#define _XTAL_FREQ 64000000 // 使用するPICにより動作周波数値を設定する
と書き換えて有り
ｓｋＩ２Ｃｌｉｂ．ｈ／ｓｋＩ２Ｃｌｉｂ．ｃ
このライブラリはI2Cデバイス(RTC/EEPROM等)と接続を行う為の関数集です。
この内容も「秋月電子Ｉ２Ｃ接続小型ＬＣＤモジュールに表示を行う」を参照下さい。
但し、CPUクロック64MHz仕様に書き換えて有り。
ｓｋＵＡＲＴｌｉｂ．ｈ／ｓｋＵＡＲＴｌｉｂ．ｃ
このライブラリはUARTデバイスとシリアル通信を行う為の関数集です。
この内容は”FT232RL使用によりUSBをシリアル変換してマイコンと繋ぎます”を参照下さい。
ｍｉｌｌｉｓ．ｈ／ｍｉｌｌｉｓ．ｃ
プログラムの実行を開始した時(起動)から現在までの時間をミリ秒単位で返す関数です。
この内容は”こちら”を参照下さい。
但し、TIMER0の設定はCPUクロック64MHz仕様に書き換えて有り。
［次ページ"カルマンフィルターの検証"へ］
リンクの見直し(*1) 2020/03/17
【きむ茶工房ガレージハウス】
Copyright (C) 2006-2020 Shigehiro Kimura All Rights Reserved.