センサーからの線形データと角度データを融合する方法

私のチームと私は、エンコーダー、商用グレードのIMU、GPSセンサーを備えた屋外ロボットをセットアップしています。ロボットは基本的なタンク駆動を備えているため、エンコーダーは左右の車輪から十分にティックを供給します。IMUは、ロール、ピッチ、ヨー、およびx、y、zの線形加速度を与えます。後で冗長性を与える他のIMUを追加できますが、ロール、ピッチ、ヨーの角速度も追加で提供できます。GPSは、グローバルなx、y、およびz座標を公開します。

ロボットのxy位置と方向を知ることは、ロボットがその環境をローカライズおよびマップしてナビゲートするのに役立ちます。ロボットの速度は、スムーズな動きの決定にも役立ちます。地上のロボットなので、z軸についてはあまり気にしません。ロボットにはLIDARセンサーとカメラもあります。したがって、ロールとピッチはLIDARとカメラのデータを変換して向きを変えるのに役立ちます。

すべてのセンサーの精度を最適に活用する方法で、これらすべての数値を融合する方法を見つけようとしています。現在、カルマンフィルターを使用[x, x-vel, x-accel, y, y-vel, y-accel]して、単純な遷移行列の推定値を生成しています。

[[1, dt, .5*dt*dt, 0,  0,        0],
 [0,  1,       dt, 0,  0,        0],
 [0,  0,        1, 0,  0,        0],
 [0,  0,        0, 1, dt, .5*dt*dt],
 [0,  0,        0, 0,  1,       dt],
 [0,  0,        0, 0,  0,        1]]

フィルタは、IMUによって提供される加速度にのみ基づいて状態を推定します。（IMUは最高の品質ではありません。約30秒以内に、ロボットが（静止状態で）初期位置から20メートルほどドリフトするのが表示されます。）ロール、ピッチ、ヨーの使用方法を知りたいIMU、および潜在的にロール、ピッチ、およびヨーレート、車輪からのエンコーダデータ、および状態推定を改善するためのGPSデータ。

少しの数学を使用して、2つのエンコーダーを使用して、ロボットのx、y、およびヘッディング情報、および線速度と角速度を生成できます。エンコーダーは非常に正確ですが、屋外のフィールドで滑りやすくなります。

ここには、融合するのが難しい2つの別個のデータセットがあるように思えます。

1. x、x-vel、x-accel、y、y-vel、y-accelの推定
2. ロール、ピッチ、ヨーの推定値、およびロール、ピッチ、ヨーの速度

これら2つのセットの間にはクロスオーバーがありますが、それらをどのように組み合わせるかについて推論するのに苦労しています。たとえば、ロボットが一定の速度で移動している場合、x-velおよびy-velによって決定されるロボットの方向は、ヨーと同じになります。ただし、ロボットが停止している場合、ヨーはxおよびyの速度によって正確に決定できません。また、角速度に変換されたエンコーダーによって提供されるデータは、ヨーレートの更新になる可能性があります...

12個の数字すべてを同じフィルターに入れるのは理にかなっていますか、それとも通常は別々に保持されていますか？このタイプの問題に対処するための十分に発達した方法はすでにありますか？

二つのこと。

1. マッピングを行う場合、本格的な同時ローカリゼーションとマッピング（SLAM）アルゴリズムが必要です。参照：同時ローカリゼーションとマッピング（SLAM）：パートI不可欠なアルゴリズム。SLAMでは、ロボットの状態の推定は問題の半分にすぎません。それを行う方法は、ここで答えられるよりも大きな質問です。

2. ローカリゼーション（ロボットの状態の推定）に関しては、これはカルマンフィルターの仕事ではありません。移行 にX （T + 1 $x(t)=[x,y,\dot{x},\dot{y},\theta,\dot{\theta}]$$x(t+1)$角加速度と角速度のため、線形関数ではありません。したがって、このタスクでは非線形推定器を検討する必要があります。はい、これを行う標準的な方法があります。はい、それらは文献で入手可能です。はい、通常、すべての入力は同じフィルターに入れられます。ロボットの位置、速度、方向、角速度が出力として使用されます。そして、はい、ここで彼らの共通のテーマの簡単な紹介をします。主なポイントは

   1. 州にジャイロとIMUのバイアスを含めるか、見積もりが分岐する
   2. 拡張カルマンフィルタ（EKF）は、一般的にこの問題のために使用されています
   3. 実装はゼロから派生させることができ、一般に「ルックアップ」する必要はありません。
   4. ローカライズとSLAMの問題のほとんどに実装が存在するため、必要以上の作業を行わないでください。参照：ロボットオペレーティングシステムROS

次に、システムのコンテキストでEKFを説明します。IMU +ジャイロ、GPS、走行距離計があります。問題のロボットは、前述のように差動ドライブです。フィルタリングタスクは、ロボットの現在の姿勢推定取ることで 、X T、制御入力U 、T、および各センサからの測定値Z Tを、次の時間ステップでの推定値を生成する X T + 1。IMU測定値をI t、GPSをG t、走行距離計をO tと呼びます。${\hat{x}_t}$$u_t$$z_t$$\hat{x}_{t+1}$$I_t$$G_t$$O_t$

私たちは次のようにロボットの姿勢を推定することに興味があると仮定し 。IMUとジャイロの問題はドリフトです。加速度には非定常的なバイアスがあり、EKFで考慮する必要があります。これは、バイアスを推定状態にすることで（通常）行われます。これにより、各タイムステップでバイアスを直接推定できます。 X T = X 、Y 、˙ X、˙ Y、θ 、˙ θ、$x_t={x,y,\dot{x},\dot{y},\theta,\dot{\theta}}$$x_t={x,y,\dot{x},\dot{y},\theta,\dot{\theta},b}$、バイアスベクトルの場合。$b$

私は仮定しています：

1. =トレッドが少しの時間増分で移動した距離を表す2つの距離測定値$O_t$
2. 3回の方向の測定は= α 、β 、θ三回のaccelleration測定 ¨ X、¨ Y、¨ Z。$I_t$${\alpha, \beta, \theta}$${\ddot{x},\ddot{y},\ddot{z}}$
3. =ロボットの位置グローバルフレーム、 G 、X 、T 、GとYのT。$G_t$$^Gx_t,^Gy_t$

通常、制御入力の結果（各トレッドの望ましい速度）を出力（ロボットの姿勢の変化）にマッピングすることは困難です。代わりに、コントロールの「結果」として走行距離を使用するのが一般的です（Thrun、走行距離の質問を参照）。この仮定は、ほとんど摩擦のない表面にいないときにうまく機能します。IMUとGPSは、後で説明するように、ずれを修正するのに役立ちます。$u$

最初のタスクは、現在の状態から次の状態を予測することであるので、 X T + 1 = fは（X、T、U 、T）。差動駆動ロボットの場合には、この予測は、（参照文献から直接得ることができるホイル式移動ロボットの運動学上または任意の近代的なロボット工学の教科書に、より簡潔な処理）、又はここに示されているように最初から誘導される：オドメトリ質問。$\hat{x}_{t+1} = f(\hat{x}_t,u_t)$

そこで、我々は今、予測することができ、X、T + 1 = fは（X、T、O トン）。これは、伝播または予測ステップです。あなたは、することができ、単純に伝播することにより、ロボットを動作させます。値ならばO tは完全に正確である、あなたは推定していないだろうX正確にあなたの本当の状態を等しくありません。これは実際には決して起こりません。$\hat{x}_{t+1} = f(\hat{x}_t, O_t)$$O_t$$\hat{x}$

これは、前の推定値からの予測値を提供するだけであり、推定値の精度が時間とともにどのように低下​​するかを教えてくれません。そのため、不確実性を伝播するには、EKF方程式（ガウスノイズの仮定の下で閉じた形で不確実性を伝播する）、パーティクルフィルター（サンプリングベースのアプローチを使用）*、UKF（ポイントワイズを使用不確実性の近似）、または他の多くのバリエーションの1つ。

$P_t$$f$$t$$P_t$$U_t$$2\times2$$.1\times I_{2\times 2}$$f$ $F_x=\frac{\partial f}{\partial x}$$F_u=\frac{\partial f}{\partial u}$

$$P_{t+1}=F_x*P_t*F_x^T + F_u*U_t*F_u^T$$

$I_t$$G_t$

$h_g()$$h_i()$$R$$R_g$$R_i$$h$

$s$$z_s$

$$v_s=z_s- h_s(\hat{x}_{t+1})$$ $$S_s = H_s*P_{t+1}*H_s^T + R_s$$ $$K = P_{t+1}*H_s^T S^{-1}_s$$ $$\hat{x}_{t+1} = \hat{x}_{t+1} - K*v$$ $$P_{t+1} = (I-K*H_s)*P_{t+1}$$

$z_g=h_g()$$H_g$$R_g$

$z_i=h_i()$$p_i$$R_i$$p_i$。バイアスの更新を組み込むことはより難しく、私の専門知識からです。ただし、平面運動に興味があるので、おそらく問題を単純化できます。これについては文献を調べる必要があります。

いくつかの私の頭の上の参照：

1. EKFベースの視覚慣性オドメトリの精度の向上

2. マルチロボット協調ローカリゼーションのための可観測性ベースの一貫したEKF推定量

3. 未知の障害バイアスを持つINS-GPS疎結合システム用の適応2段階EKF

このフィールドは十分に成熟しており、おそらくGoogle（学者）が実用的な実装を見つけることができます。この分野で多くの仕事をするつもりなら、しっかりした教科書を手に入れることをお勧めします。Google Carの名声のS. Thrunによる確率的ロボティクスのようなものかもしれません。（これらの深夜の実装に役立つリファレンスを見つけました）。

* Robot Operating System（ROS）には、PFベースの推定器がいくつかあります 。ただし、これらは屋内での使用に最適化されています。パーティクルフィルターは、マップベースのローカリゼーション（このドアまたはそのドアの近くのI）から生じる可能性があるマルチモーダルPDFを処理します。ほとんどの屋外実装（特に、少なくとも断続的にGPSを使用できる実装）では、拡張カルマンフィルター（EKF）を使用すると考えています。差動駆動の屋外の地上ローバーに拡張カルマンフィルターを使用することに成功しました。

最も一般的なケースでは、問題を大幅に簡素化できます。

• 多くの「商用グレード」のIMus（Xsensなど）には非常にノイズの多い加速度計があります。速度を得るためにそれらを融合することさえしないでください。走行距離計はすでに桁違いに優れています。IMUが提供する唯一の使用可能なデータは、ピッチとロール、およびある程度ヘディングです（次のポイントを参照）
• IMUから向かうことはそれほど信頼できません。磁気計を使用し、建物の壁で見られるような強磁性体の近くで大きなドリフト（この例では2mで最大25度）を示します。これを解決するために行ったのは、IMUヘディングを使用することですが、ヘディングバイアスを推定することです。
• 屋外にいる場合は、10度の傾斜で10m走行しても、平坦な地形で10m走行するのと同じようにXとYが変化しないことを忘れないでください。これは通常、Zを推定することによって説明されますが、別の方法で推定できると思います。
• GPSは、一般的に高マルチパス環境では嘘をつきます。さらに、低品位（および場合によっては高品位）のGPSは、非常に誤った標準偏差を報告する傾向があります。いくつかの簡単なカイ二乗検定を使用して、特定のGPS測定を統合する必要があるかどうかを確認し（つまり、特定のポイントまでの現在のフィルター推定値と一致するかどうかを確認します）、適切な結果が得られました。

私たちにとっての「典型的な」解決策は、オドメトリー+ IMUを使用して自我運動の推定値を取得し、GPSを使用してX、Y、Zおよびヘッディングバイアスを修正することです。

これは、広範囲に使用したEKF実装です。IMUの方向を推定する必要がある場合（つまり、組み込みのフィルターがまだない場合）、これら2つのフィルターUKFとEKFのいずれかを使用することもできます。