高速屋内測位システム：IMUと超音波のセンサーフュージョン
概要
屋内測位システムやGPSの一般的な位置更新レートは約8〜16Hzで、大半の産業用途には十分ですが、すべての用途に十分というわけではありません。
位置更新レートを 100Hz 以上に高める実用的な方法は、IMUと超音波のセンサーフュージョンを用いることです。センサーフュージョンは両方のデータソースの長所、つまりIMUの非常に高速な更新レートと堅牢性、そして超音波屋内測位システムの絶対座標とドリフトのなさを組み合わせます。
センサーフュージョンを用いた高速屋内測位システムの代表的な用途：
- 高速で動く移動体：例えばレーシングカートや動きの速い産業機械
- 高速スポーツ：ハーフパイプのスキージャンプ、屋内バイクパーク、BMXコース、スケートパークなど
はじめに
一般的な GPS ソリューションの更新レートは約8〜16Hz です。通常の走行には十分ですが、例えばハーフパイプのスキーヤーのジャンプや、レーシングカートの素早いターンには十分ではありません。
当社の超音波式屋内測位システム – 屋内「GPS」も同様です：
- 更新レートは約8Hzです
- 更新レートを制限している第一の要因は、超音波の飛行時間です。そのため、例えば20mのサブマップでは、理論上でも(340m/s)/20m=17Hzを超えることはできません。34mの場合 => 10Hz
- さらに、フィルタリング、無線インターフェース、その他の計算といった時間のかかる処理があり、更新レートはさらに下がります
その結果、超音波のみで測位する場合、一般的な大きさ（10〜15m）のサブマップでの標準的な更新レートは約8Hz です。
更新レート向上の制約
最大限の更新レートを実現するには、次の記事をご覧ください：位置更新レートを高め、レイテンシを低減するには？
この要件に合わせてシステムを個別にチューニングすれば、例えばフィルタリングのレベルや、場合によっては精度と引き換えに、更新レートをさらに高めることができます。しかし、主な制約要因である超音波の飛行時間は、サブマップのサイズを小さくしない限り取り除くことができません。
エアホッケーのような2〜3メートルの非常に小さなサブマップでは、最大 40Hz の更新レートが可能です。ただし、これは非常に特殊なケースです。
位置更新レートを高める方法
位置更新レートを高める方法はいくつかあります：
1) DashboardのRealtime Playerを使い、ポストプロセッシングで補間する：
- 非常に簡単ですが、この方法は文字どおり「角を削って」しまいます。既存の超音波測位データを使い、点と点の間を補間するためです。位置更新レートに比べて急なターンや速い動きがあった場合、システムはそれを知ることができず、復元することもできません
- 標準の更新レート：100Hz。それ以上はご要望に応じて対応
- 超音波による位置更新のポストプロセッシングなので、100Hzでも1kHzでもほぼ同じように簡単です
- 1kHzの更新レートでポストプロセッシングしても、100Hzと比べて精度が上がることはありません
2) Dashboardでセンサーフュージョンのポストプロセッシングを行う：
- 複数のデータソースを使うため、本当の意味でのセンサーフュージョンです
- 代表的なセンサーフュージョンはIMUと超音波の組み合わせですが、オドメトリ + 超音波（ロボット、AGV、フォークリフト）やビジュアル + 超音波（VRとAR）も可能です。超音波 + IMU + オドメーター + ビジュアルのように、複数のセンサーを融合することもできます。当社でも次のロボットで使用しています：Boxie
- 通常100Hzの更新レートで高速なIMUデータを転送する必要があるため、かなり複雑なソリューションです。モバイルビーコンが複数ある場合、無線トラフィックは10〜100倍に増え、カート側とメインコンピューター側の両方に追加のモデムを設置する必要があります
- トラッキングで角が削られることはありません。非常に高精度で、同時に非常に高速です
- 実際の標準的な更新レート（上記の方法1）のような補間ではありません）：100Hz。必要に応じて200Hzや400Hzも可能です
- モバイルビーコン側の追加モデム。この場合はRadio Head Remote（RHR）と呼ばれます。物理的には、専用ソフトウェアを搭載したModem v5.1のハードウェアです。モバイルビーコン1台につき1台
- Dashboard/コンピューター側の追加モデム。この場合はRadio Head USB（RHU）と呼ばれます。物理的には、専用ソフトウェアを搭載したModem v5.1のハードウェアです。こちらもモバイルビーコン1台につき1台です。RHRとRHUは、比較的低速な超音波位置データとは別に、高速なIMUデータ用の無線伝送チャネルを並行して構成します。超音波位置データの送信経路：通常のビーコン-モデム間無線チャネル
- IMU + 超音波センサーフュージョン – ポストプロセッシングを有効にする専用ソフトウェアライセンス。移動体1台につき1ライセンス
3) センサーフュージョンを外部コンピューターまたは搭載機器上でリアルタイム処理する：
- 方法2）と同様ですが、処理はリアルタイムで行われます
- 主な違いは、方法2）では長い走行の場合、位置データのレイテンシが数秒から数分にもなりうることです。この方法3）では、100Hzの位置更新レートでレイテンシは約15msです
- 現在利用可能：リアルタイムの超音波 + IMUセンサーフュージョンの対応アーキテクチャはIA (IAでのIMUセンサーフュージョン) およびNIAとMF NIA (NIAでのIMUセンサーフュージョン)
IMU + 超音波センサーフュージョンのポストプロセッシング
IMU + 超音波センサーフュージョンのポストプロセッシングは、2つの位置データソースを組み合わせるため、本当の意味でのセンサーフュージョンの仕組みです：
- 超音波
- IMU
慣性式および超音波式屋内測位システムの長所と短所
位置データの各ソースには、それぞれ長所と短所があります：
超音波式屋内測位システムの長所：
- 絶対位置データが得られる
- ドリフトがない
- ノイズや見通し外（超音波が直接届かない状態）により位置が飛ぶことがある
- 位置更新が比較的遅い（通常は約8Hz）
- 位置更新が速い、または非常に速い。標準は100Hz。今後のリリースでは、ご要望に応じて200〜400Hz
- 短時間の見通し外による位置の飛びがない
- 加速度センサーのデータを2回積分して算出した位置は、非常に速くドリフトします。現在のMEMS IMUだけで単独の慣性測位システムを作ることは、ほぼ不可能です。そのようなシステムは、ドリフトを除去できる超音波などのデータソースとのセンサーフュージョンに頼る必要があります
- 相対位置データしか得られない
センサーフュージョンの仕組み
では、センサーフュージョンが実際にどのように機能するかを見てみましょう。
処理の概要：
- 超音波式屋内測位システムによる測位軌跡があります。この軌跡のデータは、センサーフュージョンの有無にかかわらず同じ方法で出力されます。詳しくは、次をご覧ください：屋内測位システムのアーキテクチャ
- 超音波の測位軌跡と並行して、IMUデータ（3D加速度センサーと3Dジャイロスコープの生データ）の高速ストリームが、100Hzの更新レートで、モバイルビーコンからRHRとRHUを経由して中央のコンピューターへ送られます
- 超音波とIMUの両方のストリームにはタイムスタンプが付いており、両者を正確かつ正しく組み合わせることができます
- システムは両方のストリームを取り込み、数学的な演算、つまり実質的なセンサーフュージョンを行います
センサーフュージョンの結果
超音波による測位軌跡 – 長方形
以下の軌跡の特徴：
- 位置更新レート約8Hz
- スケール：点の間隔5cm
- 超音波のみ
- モバイルビーコンを非常に速く動かしている
センサーフュージョンによる測位軌跡 – 長方形
以下の軌跡の特徴：
- 結果：IMUとのセンサーフュージョン後の位置更新レート100 Hz – オレンジ色の点
- 元データ：超音波の位置更新レート約8Hz – 青色の点
- スケール：点の間隔5cm
- モバイルビーコンを非常に速く動かしている
軌跡は補間ではない本物の100Hzになり、同時に短時間の障害物に対しても堅牢になりました
融合されたIMU（オレンジ）と超音波（青）の位置点の拡大図
角の手前でIMUの点が減速し、角を過ぎると加速する様子がわかります。
超音波による測位軌跡 – 三角形
条件は上の例と同じですが、性能を示すために軌跡の形を変えています。
センサーフュージョンによる測位軌跡 – 三角形
融合されたIMU（オレンジ）と超音波（青）の位置点の拡大図 – 三角形の角
センサーフュージョンとRealtime Playerの比較
超音波データが不足すると、Realtime Playerは「角を削る」
超音波の測位点が少なすぎる場合（物体が速すぎる、または位置更新の頻度が低すぎる場合）、Realtime Playerは明らかに「角を削って」しまう点にご注意ください。その結果、測位誤差が大きくなります。
- 移動体を非常に速く動かしている
- 超音波の更新レート約8Hz – 青色の点
- IMUセンサーフュージョン – 100Hz – オレンジ色の点
- Realtime Player – 100Hz – ピンク色の点
Realtime Playerの結果（設定：前方0、後方5）
Realtime Playerの結果（設定：前方1、後方5）
Realtime Playerの結果（設定：前方3、後方5）
- 必要とする前方の測位点が多いほど、前方の点がないためにRealtime Playerが軌跡を描けない区間が長くなります
- 全体の点数が多いほど、軌跡は滑らかになり、位置点の欠落にも強くなりますが、同時に角もより大きく削られます。
滑らかな曲線ならRealtime Playerで十分な場合も
更新レートが十分に高ければ、Realtime Playerで良好な結果が得られる
ポストプロセッシングの所要時間
現在提供しているIMU + 超音波センサーフュージョン – ポストプロセッシング機能の処理速度は、およそ「10秒 + データ1秒あたり0.12秒」です。例えば：
- 5秒間の高速なジャンプのポストプロセッシングは、10+5*0.12 => 10.6秒で完了します
- 15分間のカート走行のポストプロセッシングには、10+15*60*0.12 => 2分かかります
上記の数値は、Win11とIntel i5を搭載したノートPCで計算した結果です。より高速なデスクトップPCであれば、はるかに短時間で処理結果が得られます。
今後のコード最適化により、新しいハードウェアや新しいライセンスを必要とせずに、ポストプロセッシングの時間が大幅に短縮される可能性があります。
位置と方向のセンサーフュージョンを混同しないでください
測位のためのセンサーフュージョンは複雑な課題です。慣性測位システムでは、位置を加速度センサーのデータの2回積分によって算出するためです。追跡対象の移動体の実際の加速度と、地球の重力の影響を明確に区別するのは容易ではありません。そのため、センサーフュージョンなしの慣性式屋内測位システムは機能しません。
一方、角度のためのセンサーフュージョンは、はるかに扱いやすい課題です。当社はこの機能を何年も前から製品として提供しています。詳細：
まとめ
センサーフュージョンは複雑な機能ですが、複数のソースのデータを効率よく組み合わせるため、高精度屋内測位システムでより高い性能を引き出します。
例えば、単独の屋内測位システムでは更新レートが通常約8Hzであるのに対し、IMUセンサーフュージョンを用いた屋内測位システムは、更新レート 100Hz を実現します。しかも、IMUセンサーフュージョンなしのシステムと同等以上の精度を保ちつつ、短時間の見通し外や外部の音響ノイズに対する堅牢性もさらに高まります。
この Realtime Player は、滑らかな曲線や、曲線の鋭さに比べて更新レートが十分に高い曲線であれば、十分に良好な結果を出します。