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

〔2/2〕 〔マイコンのトップに戻る〕


カルマンフィルター(Kalman Filter)については次ページ(2/2)で記述します。
今回のセンサーは、3軸加速度センサーADXL3453軸ジャイロセンサーL3GD20を使います。
両方ともI2CかSPIで接続しますが、今回はI2Cで行います。
3軸加速度センサーADXL345の事は、”3軸加速度センサで傾斜角度を測定してみます”を参照
して下さい。
このページでまずは、3軸ジャイロセンサーL3GD20からデータを読み出して見ます。

《3軸ジャイロセンサー》

下の図は、ジャイロセンサー(左)と加速度センサー(右)の各軸における動作方向の概念図です。

ジャイロセンサーは、各軸の回転方向の加速度が得られます、なのでセンサーが停止している時と
定速度の場合のセンサー値は0dpsとなり、動作している時のみ角速度(角加速度)が得られます。
なのでぇ、何方かの方向にどの位の速さ(加速)で傾き中(回転中)と言う事が解ります。

加速度センサーの各軸は、重力方向の傾斜角度が得られます、
なのでぇ、水平状態であればXY軸は0度で、Z軸は重力方向に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機能有り。(今回未実験)

レジスタの構成等の説明は省きます、データシートを参照下さい。

(DataDisplayで波形表示)

PICからUSARTで送られてきたデータを下図の様に波形を表示させるパソコンのソフトです。
このソフトはシリアル通信を通じてデータを送ればグラフを描きます。
なので、マイコンのADCでデータを読んでシリアルで送れば波形を表示出来ると言う物で、
3チャンネルまで対応しています。

こちらのVecterからダウンロードして下さい、インストールは不要です、
解凍したら"DataDisplay.exe"を実行するだけです。
古いソフトらしく動作環境が、”Windows XP/Me/2000/98”で、私のWindows7は動作しました。

起動させたら「ポートの設定画面」が出るので、”COMポート”と”ボーレート(19200bps)”を設定します。
グラフ画面が表示されたら、画面の右上に「データビット長:」と有りますね、
これを"10Bit"に設定します。

DataDisplay.exeの有るフォルダーに"DATALOG"フォルダーが有ります、これは表示したデータを
自動的に保存しています、なのですがこれを削除する機能がないので不要なら定期的に手動で削除
しないとファイルは溜まるだけですので注意が必要です。


下の図は、加速度センサーの各軸をPCに送り、それぞれの方向に上下左右動かしてみた画面です、
Z軸は既に1Gの加速度がかかっていますね。(センサー値そのままの値です)

加速度センサの出力波形


下の図は、ジャイロセンサーの各軸の角速度(dps)を表示させています。
(センサー値から角速度に変換した値です)
角速度は、出力値に各レンジの分解能の値を掛けます。(±250dpsなら0.00875dpsを掛けます)

ジャイロセンサの出力(角速度)波形
これを表示させるサンプルは”gyro21.c”を動作させます。
この角速度で姿勢制御させるはちょっとぉ難しいでしょう、なので角速度を角度に変換します。
変換には、台形法により一定時間ごとに定期的に読込み周期毎の移動量を累積加算すると
全体の移動角度となるらしぃ。
詳しい内容は、ネットで”ジャイロセンサー 台形法”などで調べて下さい。


下の図が、角速度を積分により角度計算させて表示させた結果です。
(Z軸のみ左右に回転させています)

ジャイロセンサの出力(積分角度)波形
これを表示させるサンプルは”gyro22.c”を動作させます。
角度は出せたのですがぁ、原点が時間と共にずれて行っています(ドリフト現象)。
この弱点を克服する為にカルマンフィルターを使う事にします。
ドリフトとは
センサーが静止しているのに出力が出ると言う誤差が生まれます、
これは温度変化や静電容量に振動等のノイズで発生するらしい、
これを積算して行っているのでどんどんずれるのでしょう?

(配線について)

18F26K22のピン構成図
 左図は、18F26K22のピン構成です。

 回路電源はセンサが3.3Vなので3.3Vです。

 MSSPは2回路有りますが、今回のI2C通信は、
 SDA2/SCL2を利用して、センサとLCDに接続します。
 I2C用のプルアップ抵抗は、PIC内部のプルアップを
 使います。

 LCDのプルアップ用の半田盛りは行いません。

PCに”超小型USBシリアル変換モジュール”でデータを送信する為に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:イベント発生時に通知する為の割り込み出力
        ピン(今回は未使用)

ジャイロセンサの配線図
実態配線図です、センサーは右上が1番ピンです。
上のLCDはこちらの物を使っています、LCDは無くてもかまいませんが、
実際にどんな値が出力されているか確認した方が良いでしょう。
尚、LCDについては、”秋月電子I2C接続小型LCDモジュールに表示を行う”を参照下さい。

(波形表示プログラムについて)

今回のPIC18F26K22のCPUクロックは64MHzで動作させています。

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

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

ダウンロードファイルを解凍すると下記の様なファイル構成です。
 gyro21.c・・・・・・・・・・・本体のサンプルソースプログラム21(角速度表示用)
 gyro22c・・・・・・・・・・・・本体のサンプルソースプログラム22(積分角度表示用)
 skL3GD20I2C.c・・・・・L3GD20関数ライブラリソースファイル(I2Cバージョン)
 skL3GD20I2C.h・・・・・L3GD20ライブラリ用ヘッダファイル(I2Cバージョン)
 skI2CLCDlib.c・・・・・・I2C接続LCD関数ライブラリソースファイル
 skI2CLCDlib.h・・・・・・I2C接続LCDライブラリ用ヘッダファイル
 skI2Clib.c・・・・・・・・・・I2C通信を行う関数ソースファイル
 skI2Clib.h・・・・・・・・・・I2C通信を行う関数のヘッダファイル
 skUARTlib.c・・・・・・・・USART通信を行う関数ソースファイル
 skUARTlib.h・・・・・・・・USART通信を行う関数のヘッダファイル
 millis.c・・・・・・・・・・・・・プログラム起動時からの時間カウントを行う関数ライブラリソースファイル
 millis.h・・・・・・・・・・・・・プログラム起動時からの時間カウントを行う関数用ヘッダファイル

DataDisplay用関数について

この関数は本体のサンプルソースプログラム(gyro21.c/gyro22.c)に記述されています。

 ans = DataDisplay(kosu, d1, d2, d3)
  センサーから読込んだデータを"DataDisplay"用のデータに変換する処理
  変換データのフォーマットは、ダウンロードし解凍した「DataDisplayの使用法.pdf」を
  見て下さい。
   kosu :3チャンネルの内で、何チャンネル分のデータを送るか設定(1-3)します
   d1  :1チャンネル用のデータ(X)を指定します
   d2  :2チャンネル用のデータ(Y)を指定します
   d3  :3チャンネル用のデータ(Z)を指定します
   ans  :変換したデータの格納先アドレスを返します
  例)1チャンネル分のデータを送る場合

     int x ;

     x = analogRead(3) ;
     Serial.print(ADCdisplay(1,x,0,0)) ; // X Ch:REDでグラフ表示される
skL3GD20I2C.c

このライブラリは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が応答していない、正常なら0が返ります。

 ans = gyro_Read(*X,*Y,*Z)
  L3GD20の各軸の値を読み込む処理です。
  XYZ軸のセンサ値を読み込んで指定の変数にデータ(センサー値そのままのデータ)を返します。
   *X : X軸の値を読み込みこの変数にデータを返す
   *Y : Y軸の値を読み込みこの変数にデータを返す
   *Z : Z軸の値を読み込みこの変数にデータを返す
   ans : 0=正常 −1=読込み失敗(I2C異常)

 ans = gyro_AVEread(su,*X,*Y,*Z)
  L3GD20の各軸の値を指定の回数(su)だけ読込み平均化し、角速度に変換した値を返す処理です。
  XYZ軸のセンサ値を読み込んで指定の変数にデータを返します。
   su : 何回のデータを読み込んで単純平均化するのか個数を指定
   *X : X軸の値を読み込みこの変数にデータを返す
   *Y : Y軸の値を読み込みこの変数にデータを返す
   *Z : Z軸の値を読み込みこの変数にデータを返す
   ans : 0=正常 −1=読込み失敗(I2C異常)

skI2CLCDlib.h/skI2CLCDlib.c

I2C接続LCDにアクセス(表示)を行う為の関数集群です。
この内容はこちらの「秋月電子I2C接続小型LCDモジュールに表示を行う」を参照下さい。
但し、
#define _XTAL_FREQ 64000000 // 使用するPICにより動作周波数値を設定する
と書き換えて有り

skI2Clib.h/skI2Clib.c

このライブラリはI2Cデバイス(RTC/EEPROM等)と接続を行う為の関数集です。
この内容も「秋月電子I2C接続小型LCDモジュールに表示を行う」を参照下さい。
但し、CPUクロック64MHz仕様に書き換えて有り。

skUARTlib.h/skUARTlib.c

このライブラリはUARTデバイスとシリアル通信を行う為の関数集です。
この内容は”FT232RL使用によりUSBをシリアル変換してマイコンと繋ぎます”を参照下さい。

millis.h/millis.c

プログラムの実行を開始した時(起動)から現在までの時間をミリ秒単位で返す関数です。
この内容は”こちら”を参照下さい。
但し、TIMER0の設定はCPUクロック64MHz仕様に書き換えて有り。



[次ページ"カルマンフィルターの検証"へ]





【きむ茶工房ガレージハウス】
Copyright (C) 2006-2016 Shigehiro Kimura All Rights Reserved.