次は、二足歩行ロボットには、欠かせない姿勢推定（以降、人間的に「平衡感覚」と記載）。今回は、加速度・ジャイロセンサであるMPU6050から「I2CDev」に付属のライブラリを利用して、物体がどのような姿勢になっているかを把握する方法を実験してみたいと思います。
MPU-6050（GY-521ボード）
MPU-6050 は、3軸加速度センサー、3軸ジャイロセンサーを備えたチップでGY-521ボードに取り付けられているものを購入し、使いました。
加速度センサー
やはり仕組みを分かった上で利用したいので、加速度センサーの仕組みを調べてみました。
加速度とは、単位時間当たりの速度の変化率のことです。公式は以下になります。
| \(\displaystyle a = (v2 – v1) / t\) | \( a \) : 加速度(\( m/s^2 \)) \( v1 \) : 地点1における速度(\( m/s \)) \( v2 \) : 地点2における速度(\( m/s \)) \( t \) : aからbへの移動時間 (\( s \)) |
この加速度を測定する方法は、以下のような仕組みだそうです。
始めにa)のように加速度0の状態では錘が検出電極間の中央にいるので、検出電極と錘の間の静電容量C1とC2が等しい状態です。
そこにb)のように加速度を加えると、反対方向に慣性力が発生し、錘がY軸方向にずれます。この時、検出電極と錘の間の静電容量C1とC2に差が生じ、C1>C2となります。
この静電容量差を測定することで慣性力を検出して加速度を求めます。（下図）
この方法でX軸、Y軸、Z軸の加速度を求めることで３軸加速度センサーが出来ています。
ジャイロセンサー
ジャイロセンサーとは、日本語では角速度センサーと呼ばれます。
角速度とは、単位時間当たりの回転角度の事を言います。公式は以下になります。
| \(\displaystyle \omega = \theta / t\) | \( \omega \) : 角速度(\( rad/s \)) \( \theta \) : 移動角度(\( rad \)) \( v2 \) : 単位時間(\( s \)) |
角度をラジアン(rad)にする方法なども知る必要がありますが、深掘りしすぎるとキリがなくなってしまうので、単位時間当たりの角度の変化量をセンサーで計測すると理解しました。
この角速度を計測する仕組みは、いくつかあるようですが、もっとも一般的なのは振動式だそうです。振動式の中にもいくつかあり、以下は、加速度センサー同様に静電容量方式の仕組みです。
始めにc)のように無回転状態で、駆動電極に矩形波の電圧を加えて振動子をX軸方向に一定周期で振動させておきます。この時、検出電極と振動子の間の静電容量C1とC2が等しい状態であるとします。
そこにd)のように回転を加えると、Y軸方向にコリオリ力が発生し、振動子がY軸方向にずれます。
この時、検出電極と振動子の間の静電容量C1とC2に差が生じ、C1≠C2となります。
この静電容量差を測定することでコリオリ力を検出して角速度を求めます。
この方法で各軸の角速度を求めることで３軸ジャイロセンサーが成り立ちます。ちなみに角速度の場合は、各軸中心の角速度をX軸はロール、Y軸はピッチ、Z軸はヨーと呼びます。
配線してみる
【余談】
そろそろちゃんと回路図を載せれるようになりたいのですが、まだしっくりくるツールを見つけることが出来ず、写真で載せています。Fritzingは有料、Wokiwはサブスク、Thinkercadは無料ですがパーツが少なそうと絞り込めていません。
いきなり変な物体にMPU6050(GY-521)が取り付けられていると驚かれた方は、以下の記事で右脚の足の付根(太腿)部分に姿勢制御の為に取り付けています。
回路は、GY-521でレギュレータが入っているとのことで5V(3.3Vでも大丈夫)をインプットにします。A5とSCL、A4とSDAを繋ぎます。今回は繋いでいませんが、ArduinoのD2とGY-521のINTも繋ぐことが多いようです。
| Arduino UNO R3 | MPU6050(GY-521) |
|---|---|
| 5V | VCC |
| GND | GND |
| A5(SCL) | SCL |
| A4(SDA) | SDA |
| (D2) | (INT) |
Arduino UNO R3 は、Raspberry Pi 4ModelB に繋いでいます。
ライブラリの準備
今回は、有名なライブラリ(I2CDev)を全面的に利用します。なので長々とPythonソースは載せません。しかし、案外最後のTeapotデモは苦労したので手順などを載せておきます。
まずは、I2CDevlibの取得です。以下のサイトで取得できます。
https://github.com/jrowberg/i2cdevlibサイトにアクセスしたらCode→Download ZIP を行います。
ダウンロードするとDownloadsの中に「i2cdevlib-master.zip」が出来上がります。このファイルを展開します。
展開すると沢山のディレクトリやファイルが解凍されますが、必要なのは、Arduino/I2CDevとArduino/MPU6050だけなので、この２つのディレクトリをコピーして、Aruduinoのlibraryディレクトリに貼り付けます。
次にArduino IDEを開いてサンプルプログラムを開きます。Arduino IDE から[ファイル]⇒[スケッチ例]⇒[MPU6050]を開いて、MPU6050_DMP6を選択します。
以下のように沢山のコメントが入ったソースコードが表示されます。（英語が苦手なので翻訳しないと解読できません）
少し走り読みしました。このサンプルソースでは、「#define」宣言のコメント化/復帰を切り替えることでいくつかのモードを切り替え出来るようです。少し長いですが、各実行モードの説明文を翻訳してみました。
- OUTPUT_READABLE_QUATERNION
実際の四元数コンポーネントを [w, x, y, z] 形式で表示したい場合は、「OUTPUT_READABLE_QUATERNION」のコメントを外してください。(ただし、Processing などのリモート ホストでの解析には最適ではありません) - OUTPUT_READABLE_EULER
FIFO からの四元数から計算されたオイラー角 (度単位) を表示したい場合は、「OUTPUT_READABLE_EULER」のコメントを解除します。 オイラー角はジンバルロックの影響を受けることに注意してください。
(詳細については、http://en.wikipedia.org/wiki/Gimbal_lock を参照してください) - OUTPUT_READABLE_YAWPITCHROLL
FIFO からのクォータニオンから計算されたヨー/ピッチ/ロール角 (度単位) を確認したい場合は、「OUTPUT_READABLE_YAWPITCHROLL」のコメントを解除します。 これには重力ベクトルの計算も必要であることに注意してください。
また、ヨー/ピッチ/ロール角はジンバル ロックの影響を受けることに注意してください。
(詳細については、http://en.wikipedia.org/wiki/Gimbal_lock を参照してください) - OUTPUT_READABLE_REALACCEL
重力が除去された加速度コンポーネントを表示したい場合は、「OUTPUT_READABLE_REALACCEL」のコメントを解除します。 この加速度参照フレームは方向に対して補正されていないため、重力の影響がなければ、+X はセンサーに従って常に +X になります。 方向に対して加速度を補正したい場合は、代わりに OUTPUT_READABLE_WORLDACCEL を使用してください。 - OUTPUT_READABLE_WORLDACCEL
重力が除去され、ワールド座標系に合わせて調整された加速度コンポーネントを表示したい場合は、「OUTPUT_READABLE_WORLDACCEL」のコメントを外します (この場合、磁力計が存在しないため、ヨーは初期方向を基準にしています)。 場合によっては非常に便利かもしれません。 - OUTPUT_TEAPOT
InvenSense ティーポットのデモに使用される形式と一致する出力が必要な場合は、「OUTPUT_TEAPOT」のコメントを解除します。
プログラムを動かしてみる
各モードのコメントアウト／復帰を切り替えて試しに動かしてみます。Arduino IDE でアップロードしてシリアルモニタを起動し、右下のbpsを115200に変更して実行します。初期値は9600bpsですが、サンプルプログラム「MPU6050_DMP6」では、setupでSerial.begin(115200)に設定されているので、シリアルモニタをそれに合わせています。
1. OUTPUT_READABLE_QUATERNION を動かしてみます。「w, x, y, z」の四元数を取得するモードです。以下のように四元数が取得されます。
2. OUTPUT_READABLE_EULER を動かしてみます。FIFOキューからの計算されたオイラー角（度単位）を表示します。
3. OUTPUT_READABLE_YAWPITCHROLL を動かしてみます。FIFO からのクォータニオンから計算されたヨー/ピッチ/ロール角 (度単位) を表示します。
4. OUTPUT_READABLE_REALACCEL を動かしてみます。重力が除去された加速度コンポーネントを表示します。ただし、方向に対して加速度を補正しません。
5. OUTPUT_READABLE_WORLDACCEL を動かしてみます。重力が除去され、ワールド座標系に合わせて調整された加速度コンポーネントを表示します。
Terpotについては、特別にProcessingを利用するので別途記載します。
MPU6050_DMP6 の Teapotデモを動かしてみる
Teapotデモだけは、シリアルモニタで確認して、「はい、終わり」というものではありません。別途、ProcessingというJavaのIDE環境を使って動かします。
まずは、Processingのインストールを行います。githubから最新版を取得して解凍し、解凍ディレクトリに移動して「./processing」と入力すれば実行されます。
$ wget https://github.com/processing/processing4/releases/download/processing-1292-4.2/processing-4.2-linux-arm64.tgz
$ tar xzvf processing-4.2-linux-arm64.tgz
$ cd processing-4.2/
$ ./processing後は、I2CDevからもってきたサンプルを動かせば良いだけですが、ここでハマりました。
まず、MPU6050_DMP6サンプルのしたにProcessingというディレクトリがあり、その中にMPUTeapotというProcessing用のソースファイルがあります。これをProcessingで開いてもコンパイルエラーになります。エラー内容は、「import toxi.geom.*;」が無効とのことです。何やら外部ライブラリが不足しているようです。「toxi」というキーワードをもとに探しました。そうするとgithubに「toxiclibs」というライブラリが見つかりました。
「toxiclibs-complate-0021.zip」をダウンロードして、~/scketchbook/libraries以下に展開します。
おそらく使っているのは、「toxiclibscore」のみですが、全てそのまま有効にしています。
Processingで外部ライブラリを有効にするには、「~/sketchbook/libraries/」配下に展開して、「ライブラリ名/library」の下のjarファイルが外部ライブラリとして認識されるようです。
再度、MPUTeapotを開いてみます。そうするメニューから[スケッチ]⇒[ライブラリをインポート…」に「toxiclibscore」が表示されるので、クリックするとMPUTeapotの先頭にライブラリのインポート文が追加されます。
最後に少し微修正、ProcessingのMPUTeapotは、シリアルデバイスを正しく認識できていません。先頭のCOMポートを採用してしまうため、Arduinoを接続したデバイスを指定する必要があります。以下のようにソースを一部修正しました。
// get a specific serial port (use EITHER this OR the specific port code below)
// String portName = Serial.list()[0];
// get a specific serial port (use EITHER this OR thiw first-available code above)
String portName = "/dev/ttyACM0";ここまでくれば、Teapotデモを動かすことが出来ました。
次回はバッテリー周りの準備、あるいは、脚の付根から腰まで組み立てるを進めていきたいと思います。
以上
コメント