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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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に設定したものです。

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

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

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

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

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

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