コンパスラグ（レート依存ヒステリシス）を処理する方法は何ですか？

距離を追跡するための低精度のホイールエンコーダと、方位を決定するための電子コンパスを備えたトレッド駆動のロボットを手に入れました。コンパスは、たとえばウェイポイントに到達した後など、ロボットが素早く旋回するときに大きな（1秒を超える）ラグがあります。

遅延に対処する方法は何ですか？多くの測定を行い、コンパス応答をモデル化できると思います。ただし、これはレートに依存し、瞬時のレートがわからないため、問題があるようです。

単純だが遅いアプローチとして、ロボットを非常に大まかに正しい方向に向けるまで回転させ、その後、正しい方向に向けるまで短時間の測定休止で非常に小さな増分回転を行います。これに対処する他の方法はありますか？

sensors compass

— ウィーンマイク
ソース

コンパスの遅れは、高周波ノイズを抑制するローパスフィルターが原因です。

ノイズが少なく、したがって遅延が少ない、より高価な磁力計が存在します。
ジャイロスコープを使用して精度を向上させることもできます。実際、これが慣性測定ユニット（IMU）の機能です。これは、カルマンフィルターを使用して実現できます。精度を上げると、遅延を減らすことができます。精度を上げると、ノイズを抑えるためのローパスフィルターへの依存が減るからです。カルマンフィルターは、磁力計からのデータとジャイロスコープ（方位の変化率を測定する）を融合します。

現在のコンパスにこだわる場合、2つの解決策があります（警告、これはますます高度になりますが、オプション1はほとんどの人があまり手間をかけずにアクセスできるはずです）。

フィルターをキャンセルすることができます。これにより遅延を除去できますが、高周波ノイズも増加します。これを行った後、ヘディングの新しい推定に基づいてロボットを制御しようとすることができます。これを行うには、ローパスフィルターのパラメーターを試してみる必要があります。たとえば、離散時間では、次のことがわかります。

時間における推測方位（コンパス出力）であり、は、時間における実際の方向（グランドトゥルース）です。
$\hat{θ} (t) = a_{0} θ (t) + a_{1} θ (t - 1) + \dots + a_{k} θ (t - k)$ $\hat\theta(t)=a_0\theta(t)+a_1\theta(t-1)+\cdots+a_k\theta(t-k)$ $\hat\theta(t)$ $t$ $\theta$ $t$
他の外部手段を使用してグラウンドトゥルースを測定する実験を行うことにより、パラメーター見つけることができます。所与のサンプルは、この式を有する $a_i$ $n+k+1$
$[\begin{matrix} \hat{θ} (k) \\ ⋮ \\ \hat{θ} (k + n) \end{matrix}] = [\begin{matrix} θ (k) & θ (k - 1) & \dots & θ (0) \\ ⋮ & ⋮ & ⋮ \\ θ (k + n) & θ (k + n - 1) & \dots & θ (n) \end{matrix}] [\begin{matrix} a_{0} \\ a_{1} \\ ⋮ \\ a_{k} \end{matrix}]$ $\left[\matrix{\hat\theta(k)\\\vdots\\\hat\theta(k+n)}\right]=\left[\matrix{\theta(k)&\theta(k-1)&\cdots&\theta(0)\\\vdots&\vdots&&\vdots\\\theta(k+n)&\theta(k+n-1)&\cdots&\theta(n)}\right]\left[\matrix{a_0\\a_1\\\vdots\\a_k}\right]$
そして、あなたは発見することによって解決することができます：
$[\begin{matrix} a_{0} \\ a_{1} \\ ⋮ \\ a_{k} \end{matrix}] = {[\begin{matrix} θ (k) & θ (k - 1) & \dots & θ (0) \\ ⋮ & ⋮ & ⋮ \\ θ (k + n) & θ (k + n - 1) & \dots & θ (n) \end{matrix}]}^{+} [\begin{matrix} \hat{θ} (k) \\ ⋮ \\ \hat{θ} (k + n) \end{matrix}]$ $\left[\matrix{a_0\\a_1\\\vdots\\a_k}\right]=\left[\matrix{\theta(k)&\theta(k-1)&\cdots&\theta(0)\\\vdots&\vdots&&\vdots\\\theta(k+n)&\theta(k+n-1)&\cdots&\theta(n)}\right]^{+}\left[\matrix{\hat\theta(k)\\\vdots\\\hat\theta(k+n)}\right]$ $M^+$ $M$ $k$ 、あなたはおそらく推測するでしょう。ボーナスポイントの場合、これはノイズが白色で独立していることを前提としていますが、最初にノイズを白色化してバイアスを除去することができるため、パラメーターの推定を改善できます。
これを伝達関数（離散時間領域でZ変換とも呼ばれます）に変換できます。

$\frac{\hat{Θ} (z)}{Θ (z)} = a_{0} + a_{1} z^{- 1} + . . . + a_{k} z^{- k}$ $\frac{\hat\Theta(z)}{\Theta(z)}=a_0+a_1 z^{-1}+...+a_k z^{-k}$
$\bar\theta(t)$

$\frac{\bar{Θ} (z)}{\hat{Θ} (z)} = \frac{1}{a_{0} + a_{1} z^{- 1} + \dots + a_{k} z^{- k}}$ $\frac{\bar\Theta(z)}{\hat\Theta(z)}=\frac{1}{a_0+a_1 z^{-1}+\cdots+a_k z^{-k}}$
時間領域に戻す：

$a_{0} \bar{θ} (t) + a_{1} \bar{θ} (t - 1) + \dots + a_{k} \bar{θ} (t - k) = \hat{θ} (t)$ $a_0\bar\theta(t)+a_1\bar\theta(t-1)+\cdots+a_k \bar\theta(t-k)=\hat\theta(t)$
$\bar{θ} (t) = \frac{\hat{θ} (t) - a_{1} \bar{θ} (t - 1) - \dots - a_{k} \bar{θ} (t - k)}{a_{0}}$ $\bar\theta(t)=\frac{\hat\theta(t)-a_1\bar\theta(t-1)-\cdots-a_k \bar\theta(t-k)}{a_0}$
$\bar\theta$

$\bar\theta$
上記の解決策はまだ最善の方法ではありません。ノイズの多い推定値はあまり有用ではありません。これを状態空間方程式に入れると、カルマンフィルター、およびLQR（線形2次レギュレータ）を使用したフルステートフィードバックコントローラーを設計できます。カルマンフィルターとLQRコントローラーの組み合わせは、LQGコントローラー（線形2次ガウス）とも呼ばれ、ループ転送回復を使用して適切なコントローラーを取得します。

これを行うには、（離散時間）状態空間方程式を考えます。

$\vec{x}(t)=A\vec{x}(t-1)+B\vec{u}(t-1)$ $\vec{y}(t)=C\vec{x}(t)$

$\vec{x} (t) = [\begin{matrix} θ (t) \\ θ (t - 1) \\ \dots \\ θ (t - k) \end{matrix}] = [\begin{matrix} A_{1} & A_{2} & \dots & 0 & 0 & 0 \\ 1 & 0 & \dots & 0 & 0 & 0 \\ 0 & 1 & \dots & 0 & 0 & 0 \\ ⋮ & ⋮ & ⋮ & ⋮ & ⋮ \\ 0 & 0 & \dots & 1 & 0 & 0 \\ 0 & 0 & \dots & 0 & 1 & 0 \end{matrix}] \vec{x} (t - 1) + [\begin{matrix} B_{0} \\ B_{1} \\ 0 \\ ⋮ \\ 0 \\ 0 \end{matrix}] \vec{u} (t - 1)$ $\vec{x}(t)=\left[\matrix{\theta(t)\\\theta(t-1)\\\cdots\\\theta(t-k)}\right]= \left[\matrix{ A_1&A_2&\cdots&0&0&0\\ 1&0&\cdots&0&0&0\\ 0&1&\cdots&0&0&0\\ \vdots&\vdots&&\vdots&\vdots&\vdots\\ 0&0&\cdots&1&0&0\\ 0&0&\cdots&0&1&0 }\right] \vec{x}(t-1) + \left[\matrix{B_0\\B_1\\0\\\vdots\\0\\0}\right]\vec{u}(t-1)$
$\vec{y} (t) = [\begin{matrix} \hat{θ} (t) \end{matrix}] = [\begin{matrix} a_{0} \\ a_{1} \\ ⋮ \\ a_{k} \end{matrix}] \vec{x} (t)$ $\vec{y}(t)=\left[\matrix{\hat\theta(t)}\right]=\left[\matrix{a_0\\a_1\\\vdots\\a_k}\right]\vec{x}(t)$
$\vec{u}(t-1)$ $A_0$ $A_1$ $B_0$ $B_1$ $B$ $A$

$Q_o$ $R_o$

その後、LQRコントローラーを設計できます。今回は、 $Q_c$ $R_c$ $Q_c = \left[\matrix{1&0&0&\cdots&0\\0&0&0&\cdots&0\\\vdots&\vdots&\vdots&&\vdots\\0&0&0&\cdots&0}\right]$ $R_c = \left[1\right]$ $J = \sum{(\vec{x}^T Q\vec{x} + \vec{u}^T R \vec{u})}$

次に、離散時間代数のリカッチ方程式を使用します。

$P = Q + A^{T} (P - P B {(R + B^{T} P B)}^{- 1} B^{T} P) A$ $P = Q + A^T \left( P - P B \left( R + B^T P B \right)^{-1} B^T P \right) A$
$P$

したがって、制御法は次のように指定できます。

$\vec{u} (t) = - K (\vec{x} (t) - {\vec{x}}_{r e f} (t))$ $\vec{u}(t)=-K(\vec{x}(t)-\vec{x}_{ref}(t))$
$K = (R + B^T P B)^{-1}(B^T P A)$

$\bar\theta$

これを修正するには、カルマンフィルターを調整し、代わりに新しい選択するループ転送回復手法を使用します。 $Q_o = Q_0 + q^2BVB^T$ $Q_0$ $Q$ $V$ $V=I$ $q$ $q \rightarrow \infty$ $Q_o$ マトリックスのチューニングが解除されるため、最適性が低下します。

$q$ $R_c$ $Q_c$

この投稿の概念はかなり高度になりますが、Riccati方程式のようなものを解く必要がある場合は、MATLABまたは他のソフトウェアを使用してこれを行うことができます。カルマンフィルターを既に実装しているライブラリもあります（もう一度、MATLABもこれを行うと思います）。

組み込みアプリケーションの場合、カルマンフィルターを自分で実装する必要がありますが、おそらくCの実装があります。

— ロナルチャン
ソース

優れた詳細な回答をありがとう。私はあなたの最初の解決策の要点に従い、私はそれを解決できると確信しています。2番目の方法は、あなたが言うように、より挑戦的であり、私はそれをすべてフォローできるかどうかを確認するために努力する必要があります。

— ViennaMike

ジャイロは簡単な答えです。ジャイロは短い測定値、コンパスは長い間聞いていました。そして、現実的には、ほとんどの場合、1杯のカルマンフィルターです。6DOFジャイロ/ ACCボードの価格は、最近では20ドル未満で、使用しないにはあまりにも安いです。

かつて、私は他の誰かのカルマンフィルターを使用していました。そしてそれを機能させた。カルマンフィルターは実際には厳密な実装ではなくアプローチに近いものであり、ジャイロの場合、最終結果は行列演算を使用する必要はありません。これにより、コードがはるかに単純になります。

— スパイク3
ソース