加速度センサ・ジャイロセンサによる姿勢検知【STM32Nucleo】

加速度センサ・ジャイロセンサ(MPU-6050)を搭載したセンサボードGY-521は 3軸加速度、 3軸ジャイロデータをインターフェースI2Cで簡単に取得でき、情報量も多く安価でお勧めなボードです。

ただし、ネット情報ではほとんどがArduino向けのもので、プログラミングは既存のライブラリ関数を使用して誰でも簡単に活用できるようになっていて仕様について理解しなくても動作させることができる反面、なんとなく使うことができてしまいますのでスキルは身につくことにはなりません。

そこで、今回の記事では加速度センサ・ジャイロセンサ(MPU-6050)の仕様書から要所を把握して、I2C通信の基本を押さえながら使いこなせるまでを解説していきます。

加速度・ジャイロセンサボード

めかのとろ

X,Y,Z3軸傾斜角をはじめから求めようとするとかなり複雑のため、この記事では1軸分(Y軸)まわりの傾斜角を加速度センサおよびジャイロセンサから算出する方法を解説します。

めかのとろ

MPU-6050では下図の向きにX、Y、Z軸を設定しています。

MPU-6050

加速度データ

めかのとろ

加速度センサでY軸まわりにθy傾斜する場合Z軸方向およびX軸方向の重力比率より傾斜角θyは逆三角関数を使用すると算出できます。

めかのとろ

加速度センサアプリでも説明しましたが、加速度センサの重力(1G)成分による各軸の値ax,ay,azは重力に対する抗力の向きとなりますので、下図のようにY軸中心に座標を正方向θY傾斜したとき、重力Z軸成分(1GZ)の抗力azは正値で、X軸成分(1GX)の抗力axは負値となります。

めかのとろ

ここでは動作による加速度は考えていませんので算出データは地上に対して傾斜させて静止した角度(絶対角度)となります。

加速度センサと動作角度
めかのとろ

算出した絶対角度にオフセットθoffset差し引くと、任意の角度で基準0°にプリセットできるので基準からの相対角度とすることができます。逆三角関数を使用した場合の角度は単位がradのため、必要に応じて°に変換します。

加速度センサによる角度の算出

ジャイロセンサデータ

めかのとろ

ジャイロセンサの場合、データgyはY軸まわりの回転時の角速度のため、角度を得るにはプログラム内で積分して算出する必要があります。

めかのとろ

まず得られた16ビットデータをスケールに応じた角速度(°/s)に変換します。それからオフセットがある場合は予め差し引いておいてから積分して角度を算出します。

ジャイロセンサによる角度算出
めかのとろ

プログラム内での積分は離散化した近似です。最もシンプルな積分は下図のアルゴリズムです。サンプリングタイムTsが十分小さい場合はこれでも問題はありません。

角速度から角度への算出
ポイント

ジャイロセンサの場合、角度は角速度を積分して算出されるために、静止した初期状態でオフセットが存在していて0°/sでなければ誤差が大きめに累積してしまいます。そのため、オフセットを予め差し引いておくと、角度のドリフトは改善されます

MPU-6050とNucleoボードとの結線図

めかのとろ

MPU-6050をNucleo-F103RBに接続して使用する場合の配線例を下図に示します。

めかのとろ

センサボードGY-521の電源にはLDOレギュレータを内蔵していますので5Vを接続します。

めかのとろ

データ出力インターフェースはI2C通信のため、STM32側ではI2C2に接続します。I2CのI/Oはオープンドレインに設定しますので通常ではプルアップ抵抗が必要ですが、センサボードGY-521には予め含まれています。

配線図

プログラミングによるデータ算出

めかのとろ

まず、はじめの関門はI2C通信の設定およびデータ読み書きを正常に実現することです。データを任意に安定して読み込めることができればあとはこれまでに解説した方法でデータを演算して角度を算出するだけです。

めかのとろ

I2C通信デバイスを使用するために、I2C2_Configuration()でまず初期設定をします。これは他のI2Cデバイスと同様です。詳細はシリアル通信I2C【STM32のI2C詳細】を参照してください。

めかのとろ

I2C2の初期設定のあとはMPU-6050の設定をします。設定はレジスタに値を書き込みます。すべてデフォルトで使用する場合は必要ありませんが、デバイスに内蔵のデジタルフィルタを使用したり、データのフルスケールを変更設定する場合には必要です。

デバイス初期設定

I2C設定とデータ書き込み・読み込み

めかのとろ

I2C通信では1バイト単位でデータを読み書きするのですが、データ読み込みでは個々のバイトデータを単独で読み込むのではなく、一度に複数バイトを読み込む方が効率的です。

めかのとろ

そうなると読み込みデータアドレスは加速度センサのX軸のデータアドレス3Bから14バイト分を読み込んでデータを配列に格納するだけでよいのです。

めかのとろ

話は前後しますが、所望のレジスタに1バイトデータを書き込むための関数は自作します。

めかのとろ

ここでのポイントスレーブアドレスで、I2Cのアドレス送信関数I2C_Send7bitAddress()ではアドレスは1バイト枠で左詰め7ビット分です。これに最下位ビットに送信か受信フラグを付加した1バイトデータとして扱います。

めかのとろ

ライブラリ関数の中身を確認して動作をたどると理解できると思います。

めかのとろ

例えばI2Cデバイスのスレーブアドレスが0x68の場合は1ビット分左シフトした0xD0を送信関数I2C_Send7bitAddress()に与えるアドレスとします。

I2C書き込み関数
めかのとろ

データ読み込みは書き込みの延長のようなものですが、複数バイトデータも連続して読み込むことができます

めかのとろ

これも自作の複数バイト読み込み関数ですが、データが複数ある場合は最後のバイトデータ受信直前でACKを返さないNACKとすることがポイントです。

I2C複数バイト読み込み関数

MPU-6050の初期設定

めかのとろ

MPU-6050 は初期設定としてレジスタへセンサへのクロック源を設定したり、加速度センサ、ジャイロセンサのフルスケールや出力へのフィルタを設定します。

めかのとろ

加速度センサおよびジャイロセンサにより角度を取得するだけならばデフォルト設定でも使えますが取扱うレジスタはせいぜい下記の程度です。

レジスタアドレス

 クロック源の指定 

めかのとろ

クロック源を指定することで MPU-6050は機能始めます。クロック源はレジスタアドレス6Bの下位3ビット分に設定します。電源投入時のデフォルトでは内部発振8MHzとなっています。

 フィルタの指定 

めかのとろ

MPU-6050には出力にノイズ対策のローパスフィルタを設定することができます。必要に応じてレジスタアドレス1Aの下位3ビットに0から6までの値を設定します。

 ジャイロセンサスケールの指定 

めかのとろ

レジスタアドレス1Bではジャイロセンサフルスケールを0から3までの値で設定します。デフォルトでは±250°/sです。

 加速度センサスケールの指定 

めかのとろ

レジスタアドレス1Cでは加速度センサフルスケールを0から3までの値で設定します。デフォルトでは±2gです。

 加速度センサデータの読み込み 

めかのとろ

加速度センサの各軸データは16ビットですが、データ格納アドレスは8ビットですので上位8ビット、下位8ビットで構成されます。X軸、Y軸、Z軸のデータはレジスタアドレス3BよりX軸上位、下位...と順に格納されます。

めかのとろ

データについては例えばフルスケールが±2gの場合は、+2g時に32768となるので1g当たり16384となります。

ジャイロセンサデータの読み込み 

めかのとろ

ジャイロセンサの各軸データも加速度センサと同様にX軸、Y軸、Z軸のデータはレジスタアドレス43よりX軸上位、下位...と順に格納されます。

めかのとろ

データについては例えばフルスケールが±250°/sの場合は、+250°/s 時に32768となるので1°/s当たり131となります。

各センサからの角度算出と相補フィルタ

めかのとろ

加速度およびジャイロセンサからのデータが取得できるようになるといよいよそれぞれのデータから角度を算出できるようになります。

めかのとろ

I2C_Read_multibyte()関数による複数バイト読み込みで格納したデータにより加速度およびジャイロセンサ各軸の16ビット値が取得できます。

めかのとろ

取得できた値により加速度センサによる角度およびジャイロセンサによる角度が算出できます。

めかのとろ

ジャイロセンサからの角速度データを積分して角度を算出するためのサンプリングタイムを正確に5msとするためにRTOSの機能を利用しています。詳細はFreeRTOSタスク管理の基本を参照してください。

加速度・ジャイロセンサから角度算出
めかのとろ

加速度センサおよびジャイロセンサから単独で算出された角度はそれぞれ一長一短あり安定しません。そこで、両センサの長所である部分をいいとこ取りするための手段に相補フィルタというものを使います。

めかのとろ

加速度センサでは重力変化による傾斜角度が得られるので特に静止状態の場合の絶対的な角度変化は信頼性がありますが、情報にノイズが含まれていたり、動作が伴うと重力以外の成分も加わることにもなります。

めかのとろ

対して、ジャイロセンサでは回転を検知した角速度が得られるので動作の伴う角度の変化には信頼性がありますが、プログラム内で積分をしているので誤差も同時に累積しドリフトを発生してしまうことになります。

めかのとろ

そこで相補フィルタを使用して、加速度センサ出力にはローパルフィルタ(LPF)により低周波部分だけをとりだし、ジャイロセンサ出力にはハイパスフィルタ(HPF)により、ドリフト分をキャンセルすることができます。

相補フィルタによる角度算出
めかのとろ

相補フィルタの式だけをみると、至ってシンプルなのですが、奥が深いので別途、相補フィルタのしくみを解明してみる【加速度・ジャイロセンサ】で詳細を解説しています。

実測結果

めかのとろ

下図は加速度センサ、ジャイロセンサおよび相補フィルタを通した出力による角度変化の様子をグラフ化したものです。横軸は経過時間(s)、縦軸は角度(°)です。

めかのとろ

加速度センサによる角度は細かなノイズが含まれていますが絶対的な角度を示すのでドリフトは発生していません

めかのとろ

ジャイロセンサによる角度はプログラム内で積分したものなのでノイズもなく滑らかですが、累積誤差によるドリフトが発生しています

めかのとろ

相補フィルタを通した出力では加速度センサによる出力とジャイロセンサによる出力の長所のみを引き継いだものになっていて安定して信頼性があります。相補フィルタのカットオフ周波数は2Hzに設定したものです。

カットオフ周波数:2Hz
めかのとろ

相補フィルタのカットオフ周波数によりどのような違いがあるのかを確認するために4Hzと2Hzで比べてみました。

めかのとろ

カットオフ周波数を4Hzに設定した場合はジャイロセンサによるドリフトの影響はないのですが、加速度センサの敏感なノイズ成分が多少含まれています。加速度センサ寄りの影響を受けた出力になっています。

カットオフ周波数:4Hz
めかのとろ

カットオフ周波数を2Hzに設定した場合は加速度センサのノイズ成分はほぼキャンセルされジャイロセンサ寄りの影響を受けた出力になっています。ジャイロセンサ出力からドリフト分だけをキャンセルした出力となっています。

カットオフ周波数:2Hz
めかのとろ

下の図はカットオフ周波数を2Hzの場合で、少し速い動作をさせてみた出力結果です。

めかのとろ

加速度センサでは速い動きほどノイズ成分が顕著に現れ、ジャイロセンサではドリフト成分によるオフセットが突然大きくなったりでこのままでは使用できません。

めかのとろ

対して、相補フィルタを通した出力ではこのような動作でも遅れは見られずしっかりと追従し、ドリフトの影響もなくほぼ理想の状態となっていてフィルタ効果が確認できます。

カットオフ周波数2Hzにおける少し速い動作
ポイント

廉価で人気のあるセンサボードGY-521(MPU-6050)をSTM32Nucleoボードに接続して傾斜角を算出する過程を解説しました。たかだか角度算出のためにと思われるかもしれませんが、ここまで踏み込んで動作を確認するとあいまいな部分が一切なくなるために、得られた知識等は新しいスキルとして自信をもって他のアプリケーションにも適用できるようになります。

コラム

STM32Nucleoで角度および角速度を取得できる手段を獲得できたところで、ようやく倒立振子を実現する道具が揃いました。現代制御理論による倒立振子のコントロールに関してはマイコンで実現するフィードバック制御のための基礎知識【番外編】で解説しましたが、近日中に実機での実現に挑戦してみたいと思います。