Quali sono i metodi per gestire il ritardo della bussola (isteresi dipendente dalla frequenza)?

12

Ho un robot guidato dal battistrada, con encoder a ruote a bassa precisione per il rilevamento della distanza e una bussola elettronica per determinare la direzione. La bussola presenta un ritardo significativo (> 1 secondo) quando il robot gira rapidamente, ad esempio dopo aver raggiunto un waypoint, ruotando in posizione per indicare la sua nuova rotta.

Quali sono i modi per affrontare il ritardo? Penso che si potrebbero fare molte misurazioni e modellare la risposta della bussola. Tuttavia, questo sembra problematico poiché dipende dalla frequenza e non conosco la frequenza istantanea.

Come approccio semplice ma lento, ho il robot che gira fino a quando non è puntato più o meno nella giusta direzione, quindi faccio piccole curve incrementali con brevi pause di misurazione fino a quando non è puntato nel modo giusto. Ci sono altri modi per affrontarlo?

sensors compass

— ViennaMike
fonte

12

Il ritardo nella bussola è dovuto a un filtro passa-basso, per sopprimere il rumore ad alta frequenza.

Esistono magnetometri più costosi che hanno meno rumore e quindi meno ritardo.
È inoltre possibile utilizzare un giroscopio per migliorare la precisione. In effetti, questo è ciò che fanno le unità di misurazione inerziale (IMU). Ciò può essere realizzato utilizzando un filtro Kalman. Il miglioramento della precisione aiuta a ridurre il ritardo, poiché una maggiore precisione riduce la dipendenza da un filtro passa basso per eliminare il rumore. Il filtro Kalman fonde i dati dal magnetometro e anche dal giroscopio (che misura la velocità di variazione della rotta).

Se rimani con la tua bussola attuale, ci sono due possibili soluzioni (Attenzione, questo diventa sempre più avanzato, ma l'opzione 1 dovrebbe essere accessibile alla maggior parte delle persone senza troppo lavoro).

Puoi provare a cancellare il filtro. Questo può rimuovere il ritardo, ma aumenta anche il rumore ad alta frequenza. Dopo aver fatto ciò, puoi provare a controllare il robot in base alla nuova stima della direzione. Per fare ciò, è necessario sperimentare per elaborare i parametri del filtro passa basso. Ad esempio, in tempo discreto, potresti trovare:

doveè il titolo stimata (uscita bussola) al tempo,è la direzione effettiva (verità di base) al momento.
$\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$
Puoi trovare i parametri facendo un esperimento in cui misuri la verità di base usando altri mezzi esterni. Dato campioni, si ha questa $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$ , quindi probabilmente indovinerai. Per i punti bonus, questo presuppone che il rumore sia bianco e indipendente, ma è possibile imbiancarlo prima per rimuovere la distorsione, e quindi migliorare la stima dei parametri.
Puoi convertirlo in una funzione di trasferimento (nota anche come Z-transform nel dominio del tempo discreto):

$\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}}$
Conversione indietro nel dominio del tempo:

$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$
La soluzione di cui sopra non è ancora il modo migliore. La stima rumorosa potrebbe non essere molto utile. Se lo inseriamo in un'equazione dello spazio degli stati, possiamo progettare un filtro Kalman e un controller di feedback a stato pieno utilizzando LQR (regolatore quadratico lineare). La combinazione di un filtro Kalman e un controller LQR è anche noto come controller LQG (gaussiano quadratico lineare) e usa il recupero del trasferimento di loop per ottenere un buon controller.

Per fare questo, crea le equazioni dello spazio degli stati (tempo discreto):

$\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$

$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})}$

Quindi, basta passare attraverso l'equazione algebrica di Riccati a tempo discreto:

$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$

Pertanto, la tua legge di controllo può essere data da:

$\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$ la matrice viene disattivata, il che significa che diventa meno ottimale.

$q$ $R_c$ $Q_c$

I concetti in questo post diventano abbastanza avanzati, ma se devi risolvere cose come l'equazione di Riccati, puoi usare MATLAB o altri software per farlo. Potrebbero esserci anche librerie che implementano già il filtro Kalman (di nuovo, credo che anche MATLAB lo faccia).

Per un'applicazione incorporata, potrebbe essere necessario implementare da soli il filtro Kalman, sebbene sia probabilmente presente un'implementazione C.

— ronalchn
fonte

Grazie per la risposta eccellente e approfondita. Seguo l'essenza della tua prima soluzione e sono sicuro di poterlo elaborare. Il secondo, come dici tu, è più impegnativo e dovrò lavorare per vedere se riesco a seguire tutto.

— ViennaMike

4

Un giroscopio è la risposta semplice. Ho sempre sentito, giroscopio per le misurazioni brevi, bussola per il lungo. E realisticamente una tazza di filtro kallman tra i due il più delle volte. Il prezzo di una scheda giroscopio 6DOF / acc in questi giorni è inferiore a $ 20, decisamente troppo economico per non usarne uno.

Un tempo, ho lavorato attraverso il filtro Kallman di qualcun altro . e ha funzionato. Un filtro kallman è in realtà più un approccio, non un'implementazione esatta, e nel caso del giroscopio, il risultato finale non ha bisogno di usare la matrice matematica. Rende il codice molto più semplice.

— Spiked3
fonte