BNO055を載せたロボットが試合中に変な方向向く人へ

• しばらく動作させてたら急に角度がジャンプしてそっぽ向くようになる
• ロボットを持ち上げたり傾けたりすると角度がズレる
• 8の字に動かしたりする儀式をしてキャリブレーションしたつもりになっている
• 角度がズレた時だけ急に強くなってオウンゴールする

上記に当てはまる人を対象とした記事です。

主にロボカップジュニア民がよく直面する問題を対象に扱いますが、BNO055を扱う人ならRCJ関係なく参考になるかもしれません。

環境

この記事を書くにあたり、RP2040マイコン + 秋月BNO055の環境で実験を行いました。この組み合わせにした理由等は特になく、手元にあったからというだけです。

ソフトウェアについては、最も使っている人が多いであろうArduino Framework + Adafruit製ライブラリを題材にして進めていきます。それ以外を使用している方も、コードの書き方が異なるだけで、解説の本質は変わりません。

改善前のプログラム

おそらく、多くの人が下記のようなプログラムを書いていると思います。ネットでBNO055 プログラムとかでググって、コピペしたらこんな感じになってるはずです。今回はこのプログラムを改善していく方法で進めます。

NDOFモードの罠

BNO055には複数の動作モードが存在します。こちらはデータシートからの引用です。

BNO055をライブラリのサンプル通りに bno.begin() で初期化すると、デフォルトで OPERATION_MODE_NDOF というモードに設定されます。このモードは9自由度のフュージョンモードで、加速度計、ジャイロ、磁力計から統合された絶対方位データが計算されます。

bno_sensor.begin();

このモードの厄介なところが、最初は起動時の方位を0°として扱う（相対方位モード）のに、磁気北を検出した瞬間にそれを基準に絶対方位モードに移行してしまう点です。この相対方位→絶対方位への移行時に、いわゆる「ロボットが急にそっぽを向く」動作をします。

多くのロボカッパーが地磁気センサではなく、ジャイロセンサを採用している理由として、会場の磁場環境が悪いことを挙げるでしょう。そもそもモーターが何個もあるフィールド上の磁場環境が良いわけありません。

そのため、地磁気情報を用いないFusionモードであるIMUモードを指定しましょう！

bno_sensor.begin(OPERATION_MODE_IMUPLUS);

Adafruitライブラリを使用していない人もOPR_MODEレジスタにxxxx1000bを指定することでIMUモードに設定できます。（データシート22ページ Table 3-5参照）

キャリブレーション

BNO055にはバックグラウンドで常に各センサのキャリブレーションを行う機能が搭載されています。データシートの51ページ 3.11に詳しく記載されていますが、要約するとこんな感じです。

オフセットを取り除くために、地磁気・ジャイロ・加速度のキャリブレーションをバックグラウンドで常にやってるよ。

• 異なる6つの姿勢にそれぞれ数秒間静止させることで加速度のキャリブレーションができるぜ。
• デバイスを安定した場所に数秒間静止させるだけでジャイロのキャリブレーションは完了するよ。
• NDOFモードでは、空中で8の字を動かしたりランダムな動きをさせることっで磁気センサのキャリブレーションができるよ。

地磁気ジャンプ現象に悩むみなさんが、ロボットの起動のたびに傾けたり8の字にロボットを動かしたりしてる理由は、予めNDOFモードのキャリブレーションを完了させて、絶対方位モードに移行させておくためだったのです。（彼らがこれを知っててやっているのか、経験則的に確立した手法なのかはさておき。）

キャリブレーションについての詳細はこちらの動画で解説されています。

残念ながら、キャリブレーションのパラメータは起動時に毎回リセットされてしまいます。そのため、予めキャリブレーションをしておき、そのパラメータを起動時にBNO055に書き込む方法が良いと思います。キャリブレーションをしておくことで、ドリフトも軽減することができます。（試合前に毎回ぐるぐるするのも不確実ですし。）

キャリブレーションの詳細

CALIB_STATレジスタで各センサのキャリブレーション状態を確認することができます。3がキャリブレーションされていて、0がダメダメであることを示しています。この値を、先の動画に示された動作をすることで全て3にすることが目標です。

サンプルプログラム

こちらにキャリブレーションプログラムを用意しておきました。こちらを実行して、シリアルモニタを見ながらキャリブレーション動作を行ってください。

最終的にこのように値が出てきたら、キャリブレーション完了です。

Calib -> Sys:2 G:3 A:3 M:3
Calib -> Sys:3 G:3 A:3 M:3
--- Offsets (copy these) ---
constexpr int16_t kAccelOffsetX = -1;
constexpr int16_t kAccelOffsetY = -14;
constexpr int16_t kAccelOffsetZ = -15;
constexpr int16_t kMagOffsetX = 160;
constexpr int16_t kMagOffsetY = 203;
constexpr int16_t kMagOffsetZ = -13;
constexpr int16_t kGyroOffsetX = -1;
constexpr int16_t kGyroOffsetY = -2;
constexpr int16_t kGyroOffsetZ = -2;
constexpr int16_t kAccelRadius = 1000;
constexpr int16_t kMagRadius = 1267;
----------------------------

これをコピペして、bno_sensor.setSensorOffsets(offsets);することでキャリブレーション値を起動時に設定することができます。こちらが最終的なプログラムです。

⚠️ 注意
キャリブレーションデータを流し込む際は、必ず一度 CONFIG_MODE に移行してから書き込む必要があります。AdafruitライブラリのsetSensorOffsets関数内ではこの切り替えが自動で行われているため、ユーザーは関数を呼ぶだけでOKですが、ライブラリを使用していない人は注意してください。

おわりに

今年こそはフィールドの横に向かってボールを運び出すロボットを見ずに済むことを祈って、この記事のおわりの言葉としたいと思います。