Jetson Nano でカルマンフィルタ不要の9軸フュージョンセンサ BOSCH BNO055を使う

こんにちは。
最近趣味でJetson Nanoや基板作りをしてるdeveminです。よろしくお願いします。

エッジAI端末として盛り上がっているJetson Nanoですが、GPIO, I2Cついててubuntu(ARM) 、tensorflow が普通に動いて最高に楽しいですね♪

さて、ツイッターにて下記のツイートをしたところ、結構反応が大きかったので、Amazonで買ったBOSCH BNO055 9軸位置推定フュージョンセンサを使った記事を書きますね。

カルマンフィルタ等しなくても通常の角度出力可能らしい BOSCH BNO055 9軸フュージョンセンサ、Amazonで買いました！レビュー等、需要レスありましたらJetson Nano実機で使って書きます。https://t.co/9W2MNJS9Z3
— でべ (@devemin) June 1, 2019

今回の内容

BNO055 概要
Jetson Nano I2C 接続で使う
Kivy 3D で使う（インストールについても）
キャリブレーションについて
出力データについて
BNO055 弱点について
もうひとつの懸念点
Jetson NanoでI2C クロック周波数を変える方法について
最後に

BNO055 概要

当センサは、ほかのジャイロセンサと違う大きな特徴があります。
ほかのジャイロセンサで多いのは、加速度・ジャイロ・コンパス等の生データを出力するのに対し、BNO055はそれらデータに加え、直接、角度を出力してくれます！

カルマンフィルタ無しで姿勢角度出力するBOSCH BNO055モジュールいたので取り急ぎ動作確認。中華製品だったけどちゃんと動いたw まだキャリブレしてないけど下記リンクのコードでさっとデータは取れた！ラズパイではbaudrate変更必要ぽいけどJetson Nanoでは今の所不要そうhttps://t.co/HRV2YqfHMm pic.twitter.com/xwGGbt1rCe
— でべ (@devemin) June 3, 2019

ですので、通常は生データを積分処理し、その後カルマンフィルタやローパスフィルタ、相補フィルタ、madgwickフィルタ等により補正を加えないと使い物にならない事が多いです。

ですが、当センサは角度を直接出力してくれますので、ソースコードがすっきりします。値段はBanggood 等で1500円程度、Amazon では1500～3000円程度と、やや上がりますが、とっつきやすさという点では、個人的には高いコスパかと思います。

当モジュールは、電源は3-5V、3軸14ビット加速度計、1秒当たり±2000度の範囲の3軸16ビットジャイロスコープ、3軸地磁気センサーを内蔵。なんとCPUにCortex ARMを内蔵しています。I2C あるいはシリアル通信にて接続できます。

Accelerometer Range +/- 4g 14bits
Gyroscope Range 2000 度/sec 16bits
Magnetometer 13/13/15 bits

モジュール単体の消費電力は最大12.3mA、ローパワーモードで2.72mA、サスペンドモードで0.04mAです。

データシートはDigikey こちらからどうぞ。
製品索引 > センサ、トランスデューサ > モーションセンサ – IMU（国際測定単位） > Bosch Sensortec BNO055
https://www.digikey.jp/product-detail/ja/bosch-sensortec/BNO055/828-1058-1-ND/6136309

Jetson Nano I2C 接続で使う

さて、それではJetson NanoのI2Cに接続して使っていきます。

（Jetson NanoでI2C 接続を利用する初期設定は、各種ドキュメントや他ページをご参照ください。単に5V,GND,SDA,SCLを接続しただけです。）

今回の中華基板 GY-BNO055 は、ロットによっては、基板上のブリッジさせるパッド部分2か所がブリッジされてない状態での出荷の物もあるようです。

中央側のパッドをブリッジする（I2C 使用）、ブリッジしない（UART 使用）
外側のパッドをブリッジする（0x29 アドレス）、ブリッジしない（0x28 アドレス）

という違いになるようなので、最初に要確認です。（ブリッジしてなくて気付かなかったことがある）

そして、そのブリッジさせる半田付けが地味に難しいです。。。もしブリッジしてないロットでしたら、そのまま半田付けできれば良いですが、短い金属線などを固定してつなげる等コツが必要かもしれません。

Pythonでデータ取得する際に利用したものは、下記リンクのソースコードです。

こちらが良さげ、ラズパイでBNO055を使うときは、baudrate周波数を変える必要があるみたい。jetson Nanoではどうなるか。明日が楽しみ！
Python module for the Adafruit BNO055 on the Raspberry Pi connected through I2C.https://t.co/HRV2YqfHMm
— でべ (@devemin) June 2, 2019

今回のAmazon 中華モジュールは、I2C アドレスが0x28から0x29になっていますので、ソース内のメインのところを、

if name == “main“:
bno = BNO055(address=0x29)

というように変更しました。これを

python3 ./BNO055.py と実行すると、

(285.125, -71.5, 158.1875)
(287.1875, -69.75, 156.0)
(289.4375, -67.875, 153.8125)
(294.0, -64.25, 150.5)
(296.9375, -62.25, 148.5)
(300.625, -60.0, 146.1875)
(305.375, -57.1875, 143.6875)
(310.75, -54.0625, 141.5)
(316.375, -50.875, 139.625)
…………

といったように出力されます。

Kivy 3D で使う（インストールについても）

（ここは寄り道です）
私は、
https://qiita.com/gotta_dive_into_python/items/b051f8a088bbdea39751
KivyでGLSL #3 Mesh

こちらの方のQiita 記事を参考にKivy にて3Dオブジェクトを回転させてみました。作ったものは下記zipです。
（Clock.schedule_intervalのところで、fps を15に下げています。また、setup_sceneで、

    self.yaw = Rotate(0, 0, 0, 1)
    self.pitch = Rotate(0, -1, 0, 0)

を加え、update_glslで

    self.rot.angle = int(-datavec[0])
    self.yaw.angle = int(datavec[1])
    self.pitch.angle = int(datavec[2])

としています。）

修正したソースは下記です。

kivy-3d ダウンロード

kivy とは、PythonでGUIアプリを簡単に作れるライブラリです。
下記ページを参照し、

https://kivy.org/doc/stable/installation/installation-linux.html#dependencies-with-sdl2

sudo add-apt-repository ppa:kivy-team/kivy
sudo apt-get update
sudo apt-get install python3-kivy
sudo apt-get install kivy-examples

そして、kivyのHello World で起動を試しますが、エラーが出るので、

~/.bashrc に、

export KIVY_GL_BACKEND=gl
export DBUS_FATAL_WARNINGS=0

を追加して、ターミナルを開きなおすと無事にKivy アプリが起動します。

キャリブレーションについて

さて、改めてBNO055の使用についてですが、どうやらこの製品はキャリブレーションは自動実行のようです。電源を入れるたびにキャリブレーションが行われるのですが、電源投入直後の、キャリブレーションしない状態でも、だいぶ良さそうなデータが出ます。

データシートやAdafruitガイド等(
https://learn.adafruit.com/adafruit-bno055-absolute-orientation-sensor?view=all )では、キャリブレーション値とRadiusを保存しておき、起動時にその値で設定しなさいという旨が書いてあります。まだここら辺は試してません。
おそらく、ACC_OFFSET_X_XXX　～　GYR_OFFSET_X_XXX　～　MAG_RADIUS_XXX あたりの一通りのデータを毎回ホストの方で保管すれば良いのでしょうかね。（データシートｐ３１～）

キャリブレーションはBOSCH公式の下記ビデオ、あるいはデータシートｐ４８を参考にします。

ソースの getCalibration で取得できるBNO055_CALIB_STAT_ADDR　のビットで、キャリブレーションが十分成されたかどうか判断できます。（下記ビデオでいうアンテナマークですかね）

まだ正確にわかってないので、ひとまずBNO055.py ソース内のデータ出力のところを、

    d1 = bno.getVector(BNO055.VECTOR_EULER)
    d2 = bno.getCalibration()
    print(str(d1) + "," + str(d2))

と変更し、データとともにキャリブレーション状態を表示するようにしてから、ビデオを参考にしながら、

①まず15度ずつくらい、ゆっくり１軸でずらす
②いったん置いて静止
③8の字にぶんまわす（優しく）

をすると、キャリブレーション状態がすべて０→３になります。（たまに1個目のSYSが変化しないのが、まだ意味不明です。。）

出力データについて

下記は、キャリブレーション実行後、静止した状態で放置したデータです。
おそらくノイズはICの方で処理されているのでしょう、かなり落ち着いたデータです。

つぎは、センサをゆっくりと振り回したデータです。かなりスムーズな連続性が認められます。

BNO055 弱点について

さて、個人的にはかなり良いと思うセンサなのですが、下記ページの方は、いくつかこのセンサに対する考察で、弱点をいくつか挙げておられます。

BOSCHの9軸センサーBNO055のオイラー角出力機能の弱点と、四元数(Quaternion)について
http://l52secondary.blog.fc2.com/blog-entry-50.html

この方のおっしゃる弱点とその対策をするには、クォータニオンで出力したデータを処理すれば解決できるとのことで、これも近々試してみたいと思います。

具体的には、さきほどのソースの中のgetQuat 関数を使ってデータを取得し処理するだけです。クォータニオンが取得できれば角度は変換計算できるとのことです。

もうひとつの懸念点

実はもうひとつ懸念点があり、先ほどのコードの作者がREADME.md で書かれていますが、
bad data (randomly fluctuating data values) coming from the device
と、I2Cクロックが速いと、データ破損が起こるとのこと。Jetson Nanoで今回データ取得した際には、まだデータがドロップする状況まで調べておりませんが、ラズパイではI2C クロック周波数を変えることで改善できるとのこと。400kHz -> 50kHz　へ。

ラズパイの場合は、

sudo modprobe -r i2c-bcm2708
sudo modprobe-i2c-bcm2708 baurate=50000

としてくださいとのこと。あるいは
/boot/config.txt のdtparamをいじれば良さそうですね。（Jetson Nanoが楽しすぎて、最近ラズパイさわってませんｗ）

Jetson NanoでI2C クロック周波数を変える方法について

Jetson Nano のI2C のボーレート変える方法わかった！

sudo su
echo 100000 > /sys/bus/i2c/devices/i2c-0/bus_clk_rate
cat /sys/bus/i2c/devices/i2c-0/bus_clk_rate

最初のsudo su しないとダメだった。なるほどね～。
これ前に出来なかった事だったから嬉しいｗ
— でべ (@devemin) December 9, 2020

これで変えられました！

Jetson Nanoの方でI2C クロックを変えるのは、まだ簡単にする方法を見つけられていません。

下記リンクでは、
~~https://t.co/xqVRJgxJSC~~
jetson TK1 でI2C クロックを変える話題がありますが、一番最後の

echo 10000 > /sys/bus/i2c/devices/i2c-<BUS>/bus_clk_rate

では、変えられませんでした。

ラズパイは動的にモジュール読み込みする仕組みがあり、先のコンフィグを変えれば再起動すれば使えますが、Jetson Nanoはそうはいかないようです。

ほかの通常通りの変更方法だと、デバイスツリーという仕組みが導入されている組み込みLinux では、そのツリーを修正することで、GPIOやピン機能の情報を変えられます。

DTB ファイルを逆コンパイル→DTSテキスト修正→DTBに戻して書き込み　という流れのようです。(カーネルソース内のflashスクリプトを使用)

しかし詳細データは2019/6/4現在、まとまった情報ではまだ公開されておらず、nVidia フォーラムにある公式スタッフ等の書き込みを見ながら手探りになります。

6月になるとnVidia からJetson Nanoの新しい情報が公開されるとのことなので、そのデバイスツリー関連は、少し待ちましょう♪

最後に

さて、だいぶ記事が長くなってしまいましたが、非常にとっつきやすくて使いやすいBOSCH BNO055、私はおススメだと思います♪

むかし、秋葉原の秋月でジャイロセンサを意気込んで買ってきた私ですが、データを取得し補正しても、あまり良いデータが取れず「チーン」となった経験があります・・・ｗ

このセンサであれば、面倒な計算はいらず、ソースコードもシンプルになり、非常に気持ちよく使えます！

（ただ、どのジャイロセンサでも同じですが、当然グチャグチャに動かしたりすれば、データはずれていきます。また、弱点を書かれた方のサイトでも書いてあるように、特定の範囲での正確性が保証されない部分があります。個性を理解して使うことが必要ですね。　子育ても仕事もセンサもディープラーニングもぜーんぶそうですよね。当記事に関係ないことですがｗ　）

Jetson Nanoは先日まで在庫があまりなかったようですが、最近は世にそろそろ流通してきたようなので、徐々にユーザーコミュニティができるのではないでしょうか。nVidiaやディープラーニング関連の勉強会もConnpassでいろいろありますので、ぜひ今後も記事を書きつつ、ほかのユーザーさんたちと情報交換していきたいなと考えておりますので、どうぞよろしくお願いいたします♪

devemin

追記

コメントにて

ほかにもMPU-9250等はDMPというクォータニオンや角度を直接取得できる機能があります。ありがとうございます！