ロボットに搭載したジャイロセンサからの角速度を取得して角度を算出した時に、予想を超えた角度のずれが出てしまいました。
原因は「前提としていたジャイロの特性値が異なっていたこと」で、結果として単純なことだったのですが、なかなか気づくことができず困りました。
その内容と解決にあたりアドバイスいただいたことについて記載します。
ロボットと搭載したジャイロIC
マイクロマウスという迷路を解く自律移動ロボットにジャイロセンサをつんでいます。
搭載したICはSTMicroelectronicsの「ISM330DHCX」。3軸ジャイロスコープと3軸加速度センサを持ち、LGAパッケージ（2.5 x 3.0 x 0.83mm）。足・はんだフィレットが外周に出ないため、フットプリント上でも小さく、パッケージサイズが最外形の素子です。
ロボットではジャイロのデジタル16bitの出力をSPI通信で受け取っており、ジャイロのレンジは選択でき最大で4000dpsです。
観察されたトラブル
ロボットに搭載されたジャイロのレンジを2000dpsに設定して、受け取ったデジタルデータを2000dpsから角速度に変換、それを制御周期で足していくことで角度を算出します。
ロボットに200度旋回する指令を入れた時の結果です。
動画のように200度を超えて約225度まで回ってしまいました。おかしい動作をしています。
原因と解決
原因は「ジャイロのレンジを2000dpsとして角速度を計算していたため」です。ジャイロの設定を2000dpsに設定しているのだから、2000dpsで角速度を計算するのは普通よね。
データシートを見てみましょう。下記のスペックが書いてあります。
レンジは2000dpsとあります。さらに感度は±2%とあり、動画のように200度目標の旋回指令に対し、約225度回ってしまい、約13%もずれているのは明らかに変と思えます。
さて、このレンジと感度に落とし穴がありました。素子のデータシート1ページ目の案内にも、レンジのTYPにも2000dpsと記載されているため、1LSBの感度はてっきり、2000dpsの16bit分解だと思っていました。
しかし、データシート記載の「感度」を使って「レンジ」を計算してみると2000dpsではなく、2293.8dpsがTYPとなります。
そうなんです。この素子ISM330DHCXの感度の中心は2000dpsに対して14.7%ずれており、ぜんぜんTYP2000dpsではない。このずれで角速度が小さくなってしまい、実世界で200度になってもロボット的にはまだ到達していないと考え、止まらずに角度がずれたというわけです。
そこで、ロボット内部の角速度[rad/s]の計算にレンジ(2000dps）を使うのではなく、感度（70mdps/LSB）をつかうようにして動かすことにしました。ロボットに180度の指令を入れた場合の動作です。
180度で止まるようになりました。
以上のように、ジャイロ素子には感度のTYPとレンジのTYPがあっていない場合があるようです。ジャイロを使う場合はデータシートに書いてある感度から考えた場合、レンジの中心がどこになるのかも確認するようにしましょう。
アドバイスいただいた観点
今回は上記で書きました感度の設定の問題で角度ずれが起きましたが、それに気づくまでに試し・確認したことがあります。可能性として考えられたため、ジャイロがうまく動かないときの切り口として確認するといいでしょう。（アドバイスいただいた方々感謝）
ICのレジスタ設定が正しいか
このジャイロはレンジを変えられます。ICのレジスタに書き込みを行い設定することになります。通信がうまくいっていない場合も考えられます。書き込んだレジスタを読み出して値が書き換わっているかを確認しましょう。
データの欠落や異常のようなデータが出ていないか
角度は角速度の和で作ることになるため、角速度のデータが途中で通信できなくなっていたり、飛んでいたりした場合、角度の値がおかしくなります。出力データを確認して、通信出来ていない等ないかを確認。
割り込みの時間が正しくないかもしれない
角度は、周期的な割り込みで角速度と取得周期から角度を算出します。取得周期が1msで、角速度が1000dps得られた場合、1msでの角度は 1000dps * 0.001s = 1 degです。ここで取得周期が1msではなくずれていた場合、算出される角度が変わってしまいます。
マイコン側の設定を間違えている可能性もありますが、回路アートワークがうまくいっておらずオシレータにノイズがのってしまい動作が不安定になっていたりするかもしれません。また実装した素子の周波数が異なっている可能性も考えられます。
ICの角速度のサンプリング周期が制御周期より遅くないか
ICからのデータ取得はマイコン側からの命令で実行されていますが、内部のレジスタの値を更新する周期はICの設定によって変わります。サンプリングする周波数が遅いと最新のデータに更新がされず、実際とデータの間のずれが大きくなるため、角度の値もずれるということです。
角速度をLSBから物理値へ変換する際のずれ（浮動小数点の情報落ち等）
角速度はデジタルデータとして整数で手に入ります。今回のICでは16bitのため「-32768～32767」の範囲で変化します。物理値を計算するとき浮動小数点に変換することになります。
単精度浮動小数点数 floatは「約+10-37～ +1038」程度を表現することができますが、この間の数字を全て正確に表しているわけではありません。そのため、浮動小数点のデータを多く足し合わせていくようなことをするとずれが大きくなっていきます。
そこでジャイロの整数データで積算し、最後に物理値への変換をしてみてずれが大きいかどうかを確認してみましょう。
浮動小数点の数値の扱われ方について詳細に書いてくれている記事を見つけましたので参考文献としてメモします。（float型では123456789すらも表現できない話）
また、物理値へ変換するときに、整数と浮動小数点を混ぜて計算している場合、計算順序でまるめられていないかも注意が必要。
その他ジャイロを使う時に考える点
他にも、ジャイロを使う上で織り込んでいる項目の一部を紹介します。
温度によるゼロレベル・ドリフトの変化
ジャイロは角速度0（静止）しているときでも、0の値を返してくれません。ある程度ばらついた値を出力してきます。その値を単純に積算していくとずれのもととなります。またそれらは電源投入後、素子の温度が変わっていくと出力が変化し、さらにずれが大きくなります。
そこでジャイロが温度でどのように変わるのかを事前に確認してセンサ値の補正を行うとより精度がよくなると思います。
感度のずれ
今回のジャイロは感度で±2%でばらつくとデータシートに書かれていました。これはセンサの特性として、実際の角速度と異なるということです。そのため角度にもずれが起きます。
このばらつきを合わせこむために、ロボットを旋回動作させ、その時の角度ずれはどれほどになるかを確認します。最後はそのずれから計算された感度のずれ分を補正するために、センサ値にゲインをかけましょう。
またこの感度ですが、右回り・左回りでそれぞれわずかに違う場合があるかもしれません。回転方向を変えて確かめてみるのもよいと思います。違いが出るのか確かかはわからないですが、私のロボットでは、同じ方向で回り続けないように、回転方向を変えるようにして動作させています。
まとめ
ジャイロで角度が合わなかった話をしました。
このロボットは前作で使っていたいくつかの素子がEOLしてしまっており、新しい素子を使ってます。前作の素子はレンジと感度のTYPがほぼ同じでずれがなかったため、あまり気にしたことがなく、上記の旋回による感度合わせを考えるレベルのずれでした。
今回は10%を超えるずれを起こしたため、さすがに感度合わせだけでは気持ち悪く、原因を探してましたが、このように明示されているレンジと感度の間に大きく乖離がある場合もあるんだなと学びましたね。
ジャイロ等感度があり、今まで使ったことない素子を使う時はレンジと感度がどれだけ違うのかを把握して使うようにしましょう。
最後ですが、Twitterでアドバイスをくださった方々ありがとうございました。
