はじめに
本記事ではSpresense マルチIMU add-onボードを用いてどのように自己位置推定を行ったかについての私的なまとめを記載しています。
具体的なコードは以下のリポジトリにアップロードされています。
背景
IMUといえば、加速度で重力方向を確認しつつジャイロで姿勢を推定できれば良くて、位置推定を行うには高価な機材を購入しないとノイズが多くて使えないというのがこれまでの常識でした。しかし、SONYのマルチIMU技術を用いることで、安い民生用IMUを組み合わせて比較的安価なIMUが作られました。それが、Spresense マルチIMU add-onボードです。実際にこのボードを用いて、所定の経路をIMUを用いた位置推定だけで周回するデモも行われていました。
一方で、デモで使用されたプログラム自体は輸出規制や外為法などに抵触するため公開されていません。そのため、ユーザ側で位置推定のプログラムを作成する必要がありました。本記事では私なりにIMU自己位置推定をどのように行ったかをまとめています。
実装の方針
Spresense マルチIMU add-onボードは比較的ノイズが小さいセンサではありますが、ノイズ=誤差ではないことに注意が必要です。通常のIMUと同様にドリフトが発生し、温度や取り付け方、コネクタに加わる外力などでセンサに加わる応力の変化からドリフトが変わるようです。また、センサの計測スケールにも誤差が発生するので、複数の姿勢で計測してキャリブレーションしなければ厳密な計測値を得ることは難しいです。ただ、いちいちキャリブレーションするのは面倒ユーザの使いやすさを考慮して、ある程度精度は犠牲にすることとしました。
自己位置推定プログラムの構成
プログラムの構成は以下のようになっており、各サブコアに処理結果を順番に渡していくような構成になっています。
最終的に得られる位置や姿勢、速度などの情報は1920Hzのまま送信することが非常に難しいので、30Hzまで間引いて送信しています。
┌─────────────┐ ┌─────────────┐ ┌─────────────┐ ┌─────────────┐ ┌─────────────┐
│ MainCore │───▶│ SubCore1 │───▶│ SubCore2 │───▶│ SubCore3 │───▶│ SubCore4 │
│ IMU読取 │ │ ガウシアン │ │ Fusion │ │ ZUPT │ │ 位置 │
│ 1920Hz │ │ フィルタ │ │ AHRS │ │ バイアス │ │ 積分 │
└─────────────┘ └─────────────┘ └─────────────┘ └─────────────┘ └──────┬──────┘
│
▼
シリアル出力 (30Hz)
次にサブコアでの処理内容についてまとめます。
1. センサデータ前処理（ガウシアンフィルタ）
この部分ではセンサの生データをガウシアンフィルタで平滑化する処理を行っています。単純な移動平均フィルタではないのは、センサデータの変化に対して素早く反応できるようにしたかったからです。計測される際の動作の周波数の範囲が既知である場合（例えば、比較的低速で動作する環境において高周波の振動を受ける場合）FIRフィルタやIIRフィルタで適当なバンドパスフィルタなどを組んであげると性能が上がる可能性はあります。
フィルタの重みについて
ガウシアンやFIR,IIRフィルタのフィルタの重みは総和が1になるように平滑化しておくと、格納されたセンサデータがリングバッファを抜けるまでの間に後段の処理に与えるデータの総量が変わらなくなるので、積分誤差が生じにくい気がします。これは以前、単純に積分して位置推定をしていたときの知見なので、AHRSなどで処理する場合にも同様かは不明です。
2. Fusion AHRS
この部分では、前処理後のセンサデータを用いてAHRSフィルタにより姿勢を推定しています。フィルタの実装には以前からお世話になっているFusionライブラリを使ってみました。単純なC言語の実装なので、Arduino IDEでも問題なくビルドできました。センサデータを入れるだけで姿勢推定だけでなく重力を考慮した加速度も求めてくれるので、非常に助かっています。
3. ZUPT(ゼロ速度ポテンシャル更新)
この部分では、現在の計測値を元にセンサが止まっているかどうかを判別して止まっている場合に現在の速度を0にリセットしつつバイアスの評価を行います。センサが止まっているかどうかは計測値の分散を用いて評価します。動いている場合には多少の揺らぎがあるので止まっている場合より分散が大きくなることを期待しています。
バイアス（ドリフト）の更新について
センサが止まっていると判断しているときにセンサのデータが0となるようにバイアスを調整することで、IMUのドリフトを抑制することができます。しかし、センサデータにはノイズがあるので、単純にセンサが静止しているときのセンサデータの残差をドリフトとしてしまうと過剰に補正してしまうことになります。そのため、残差に小さめの重みをかけることで徐々に補正するようにしています。
4. 位置積分
速度や位置を積分する場合、単純に現在の加速度や速度に処理時間をかけて積分すると誤差が大きくなるので、前回の加速度や速度を用いた台形則を使用します。以下の記事にあるように、台形則での積分誤差は分割数を上げることで減らすことができます。そのため計測周波数をできるだけ上げて単位時間あたりの分割数を増やすことで誤差の低減が期待できます。
実行例
一辺0.5mの四角形の経路を動かした際の挙動と軌跡を示します。
約2mの移動で誤差が0.05m程度の誤差になりました。
上記の実行例では約20分ほど静止させたセンサを使用しています。この待ち時間は主に温度変化によるドリフトの変化が安定するまで待つためのものです。この待ち時間は環境によって異なるとは思いますが、以下のXのポストでは2000秒くらいが目安になるとの情報もあるようです。
まとめ
本記事では、Spresense マルチIMU add-onボードを用いて自己位置推定を実装するポイントについてまとめてみました。自前でIMUの自己位置推定を組む際の参考になれば幸いです。特にセンサ前処理については必要性の有無や導入するフィルタの種類は環境によって様々だと思います。ぜひ実際に試してみてIMU単体での自己位置推定の難しさや奥深さを体感してみてください。