¿Cuáles son los métodos para tratar el retraso de la brújula (histéresis dependiente de la frecuencia)?

12

Tengo un robot accionado por la banda de rodadura, con codificadores de rueda de baja precisión para el seguimiento de la distancia y una brújula electrónica para determinar el rumbo. La brújula tiene un retraso significativo (> 1 segundo) cuando el robot gira rápidamente, por ejemplo, después de alcanzar un punto de referencia, girando en su lugar para apuntar a su nuevo rumbo.

¿Cuáles son las formas de lidiar con el retraso? Creo que uno podría tomar muchas medidas y modelar la respuesta de la brújula. Sin embargo, esto parece problemático ya que depende de la velocidad y no sé la velocidad instantánea.

Como un enfoque simple pero lento, hago que el robot gire hasta que apunte más o menos en la dirección correcta, luego hago giros incrementales muy pequeños con breves pausas de medición hasta que apunte en la dirección correcta. ¿Hay otras formas de lidiar con esto?

sensors compass

— ViennaMike
fuente

12

El retraso en la brújula se debe a un filtro de paso bajo, para suprimir el ruido de alta frecuencia.

Existen magnetómetros más caros que tienen menos ruido y, por lo tanto, menos retraso.
También es posible usar un giroscopio para mejorar la precisión. De hecho, esto es lo que hacen las Unidades de Medición Inercial (IMU). Esto se puede lograr mediante el uso de un filtro de Kalman. Mejorar la precisión ayuda a disminuir el retraso, porque una mayor precisión reduce la dependencia de un filtro de paso bajo para suprimir el ruido. El filtro Kalman fusiona los datos del magnetómetro y también el giroscopio (que mide la velocidad de cambio en el rumbo).

Si sigue con su brújula actual, hay dos soluciones posibles (Advertencia, esto se hace cada vez más avanzado, pero la opción 1 debería ser accesible para la mayoría de las personas sin demasiado trabajo).

Puedes intentar cancelar el filtro. Esto puede eliminar el retraso, pero también aumenta el ruido de alta frecuencia. Después de hacer esto, puede intentar controlar el robot en función de la nueva estimación de rumbo. Para hacer esto, debe experimentar para resolver los parámetros del filtro de paso bajo. Por ejemplo, en un tiempo discreto, puede encontrar:

donde es el título estimado (salida de brújula) en el tiempo , es el título real (verdad básica) en el tiempo .
$\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$
Puede encontrar los parámetros haciendo un experimento en el que mide la verdad básica utilizando algún otro medio externo. Dadas muestras, tiene esta ecuación: $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]$
Y puede resolverlo encontrando: , dondees la matriz pseudo-inversa de. No hay una forma definitiva de hacer ejercicio
$[\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$ , así que probablemente solo adivinarás. Para los puntos de bonificación, esto supone que el ruido es blanco e independiente, pero puede blanquearlo primero para eliminar el sesgo y, por lo tanto, mejorar su estimación de los parámetros.
Puede convertir esto en una función de transferencia (también conocida como transformación Z en el dominio de tiempo 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}}$
Convertir de nuevo al dominio del tiempo:

$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 solución anterior todavía no es la mejor manera. La estimación ruidosa puede no ser muy útil. Si ponemos esto en una ecuación de espacio de estado, podemos diseñar un filtro de Kalman y un controlador de retroalimentación de estado completo usando LQR (regulador cuadrático lineal). La combinación de un filtro Kalman y un controlador LQR también se conoce como un controlador LQG (gaussiano cuadrático lineal), y utiliza la recuperación de transferencia de bucle para obtener un buen controlador.

Para hacer esto, invente las ecuaciones de espacio de estado (tiempo 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)$
dónde $\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})}$

Luego, simplemente lo pones a través de la ecuación algebraica Riccati de tiempo 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$

Por lo tanto, su ley de control puede ser dada por:

$\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 matriz se desintoniza, lo que significa que se vuelve menos óptima.

$q$ $R_c$ $Q_c$

Los conceptos en esta publicación son bastante avanzados, pero si necesita resolver cosas como la ecuación de Riccati, puede usar MATLAB u otro software para hacer esto. También puede haber bibliotecas que ya estén implementando el filtro de Kalman (de nuevo, creo que MATLAB también hace esto).

Para una aplicación integrada, es posible que deba implementar el filtro Kalman usted mismo, aunque probablemente haya una implementación en C.

— ronalchn
fuente

Gracias por su excelente y profunda respuesta. Sigo la esencia de tu primera solución y estoy seguro de que puedo superarla. El segundo, como dices, es más desafiante y tendré que trabajar para ver si puedo seguirlo todo.

— ViennaMike

4

Un giroscopio es la respuesta simple. Siempre he escuchado, giroscopio para las mediciones cortas, brújula para las largas. Y de manera realista, una taza de filtro de Kallman entre los dos la mayor parte del tiempo. El precio de una placa de giroscopio / acc 6DOF es menos de $ 20 en estos días, demasiado barato para no usarlo.

En un momento, trabajé a través del filtro Kallman de otra persona . y lo puse a trabajar. Un filtro de Kallman es en realidad más un enfoque, no una implementación exacta, y en el caso del giroscopio, el resultado final no necesita usar matemáticas de matriz. Hace que el código sea mucho más simple.

— Claveteado3
fuente