視覚慣性走行距離計のIMU事前統合モデルについてお話しましょう

エンジニアリングの実践では、単に視覚オドメトリ (VO) を使用するのではなく、視覚と IMU を組み合わせたソリューション、つまり視覚慣性オドメトリ (VIO) を使用して動きを推定する理由について説明します。視覚慣性オドメトリのセンサーには、主にカメラと慣性計測ユニット (IMU) が含まれます。これらのセンサーにはそれぞれ長所と短所があります。VIO の利点は、IMU とカメラの相補性にあります。

ビジョンセンサーは、テクスチャが豊富なほとんどのシーンではうまく機能しますが、ガラスや白い壁などの特徴のないシーンではうまく機能しません。しかし、カメラデータの利点は、データがほとんどドリフトしないことです。カメラが固定されている場合、推定ポーズも固定されます。

IMU センサー自体にも欠点があります。たとえば、IMU を長期間使用すると、累積誤差が非常に大きくなります。しかし、短期間であれば相対変位データの精度は高いため、視覚センサーが故障した場合でも、IMU データを融合することで測位精度を向上させることができます。 IMU は角速度と加速度を測定できますが、これらの量には明らかなドリフトがあり、2 回積分して得られたデータには大きな誤差が生じます。 IMU が固定されている場合、そのデータはドリフトし、統合された姿勢がずれてしまいます。ただし、短時間の高速移動の場合、IMU は比較的正確な推定値を提供できるため、カメラの欠点を補うことができます。ぼやけは、カメラの動きが速すぎる場合、または 2 つのフレームの重なりが一致しないほど小さい場合に発生します。

IMU とカメラは互いに補完し合うため、現在の SLAM アルゴリズムの研究の多くは、姿勢推定のためにカメラと IMU データを融合することに関するものです。

IMUエラー部分

VIO について言及すると、多くの人がすぐに「事前統合」という言葉を思い浮かべると思います。これは、VIOに関わりたい研究者が最もよく目にする言葉でもあります。それでは、事前統合とは一体何なのか、そしてなぜそれが VIO においてそれほど重要なのかが、以下の議論の焦点となります。

上記の式から、読み取ったデータは客観的な事実ではなく、客観的な事実にセンサーの誤差、つまりバイアスとノイズを加えたものであることがわかります。

a と g はそれぞれ加速度とジャイロを表し、 w はワールド座標系を表し、b は IMU ボディ座標系を表します。次の数式には多くの下付き文字があり、混乱する可能性があります。

変位、速度、姿勢（位置、速度、四元数、すなわちPVQ）の時間微分は次のように表される。

最初の 2 つの式には問題はなく、3 つの運動量間の積分と微分にすぎません。 3 つ目は四元数の微分についてです。後で読みやすくするために、ここで簡単に説明します。

途中に四元数の導出についてのセクションがあるので、続けましょう。

j 番目の瞬間の PVQ は、i 番目の瞬間の PVQ からの IMU 測定値を積分することによって得られます。

IMU のサンプリング周波数は、通常 100Hz - 1000Hz と高いため、データ量が非常に多くなります。最適化する場合、状態変数にそれほど多くのデータを入れることは不可能です。そのため、通常は、1 秒ごとに 1 つ抽出するなど、ときどきデータを抽出します。つまり、上記の式では、i が最初の 1 秒間に抽出された IMU データである場合、j は 2 秒目に抽出された IMU データです。基本的なプロセスは、i 秒目の PVQ が既知であり、i 秒目と j 秒目 (100 など) の間のすべてのデータと既知の運動学的知識が i 秒目から少しずつ統合され、j 秒目の PVQ が取得されるというものです。ただし、バックエンドの最適化のプロセスで、反復ソリューション計算を実行して PVQ 値を更新および調整する場合、PVQ が調整されると (たとえば、最初の 1 秒間)、各中間プロセスと後続のすべての軌跡を再積分して再計算する必要があります。100Hz の場合、2 秒間に 100 個のデータが収集され、100 個の積分を計算する必要があります。このような面倒な統合プロセスを回避する方法はあるでしょうか?事前統合の目的は、これらの 100 の統合プロセスを 1 つの統合だけに変えること、または 100 の値を 1 つの値に置き換えることです。事前統合モデルを適用すると、計算量を大幅に節約できます。

簡単な数式で積分モデルを事前積分モデルに変換できます。

変化  IMU事前統合。それぞれ位置、速度、姿勢に対応します。

事前積分量は IMU 測定値にのみ関連しており、一定期間にわたって IMU データを直接積分して事前積分量を取得します。

下の図は、IMU とビジョン センサーのモデルを示しており、ある程度理解するのに役立ちます。

IMU カメラモデル

事前積分の離散形式

IMU 事前積分エラー

IMU によって一定期間にわたって構築された事前積分量は、2 つの瞬間間の状態量を制約するための測定値として使用されます。

誤差の変位、速度、バイアスは、測定値から推定値を差し引くことで直接減算して得られます。 2行目は四元数の回転誤差です。

積分モデルを事前積分モデルに変換すると、計算量は効果的に削減されますが、何かが失われることもあります。 (たとえば、100 個の) データ ポイントを 1 つの結果に置き換えると、この 1 つの結果の不確実性はわからなくなります。変換前に、これらの 100 個のデータ ポイントのそれぞれの不確実性はわかっています (IMU データのノイズ分散を測定値として較正できるため)。しかし、これらの 100 個のデータ ポイントを統合することによって形成される事前統合量の分散は何でしょうか。これには、IMU 事前積分の結果を取得した後に事前積分量の共分散を導出する必要があり、IMU ノイズと事前積分量の間の線形再帰関係を知る必要があります。

隣接する瞬間の誤差の線形伝達方程式が既知であると仮定します。

つまり、誤差の伝達は 2 つの部分に分かれており、現在の瞬間の誤差が次の瞬間に伝達され、現在の瞬間の測定ノイズが次の瞬間に伝達されます。

共分散行列は再帰的に計算できます。

で

上記の結果の導出には、リー群とリー代数の知識が利用されています。プロセスが複雑すぎるため、ここでは導出プロセスを詳しく説明しません。上記の結果は、主流の VIO オープンソース コード (VINS-MONO) に記述されているものと同じ式形式です。読者は、コード内の対応する部分と直接比較できます。時間があれば、上記式の導出過程を更新します。

上記は事前統合の簡単な紹介です。VIOオープンソースフレームワーク（VINS-MONOなど）では、IMU事前統合部分はフロントエンド部分に属し、IMUデータが収集された直後に実行される計算です。フロントエンドに加えて、完全な VIO システムには、初期化部分 (つまり、IMU データとカメラ データの調整) や最も重要なバックエンドの最適化とソリューション部分など、他の多くの部分があります。バックエンド最適化ソリューションには、全体の目的関数をどのように構築するか、最適化プロセスでスライディングウィンドウ法を使用して状態量の数を確保し、計算量を制御する方法など、多くの内容が含まれています。個人的な学習が進むにつれて、要約して整理する予定です。