Quais são os métodos para lidar com o atraso da bússola (histerese dependente da taxa)?

Eu tenho um robô com banda de rodagem, com codificadores de roda de baixa precisão para rastrear a distância e uma bússola eletrônica para determinar a direção. A bússola apresenta um atraso significativo (> 1 segundo) quando o robô gira rapidamente, por exemplo, após atingir um ponto de passagem - girando no local para apontar para o novo rumo.

Quais são as maneiras de lidar com o atraso? Eu acho que se poderia fazer muitas medições e modelar a resposta da bússola. No entanto, isso parece problemático, pois depende da taxa e eu não sei a taxa instantânea.

Como uma abordagem simples, mas lenta, eu tenho o robô girando até que seja apontado mais ou menos na direção certa e, em seguida, faça voltas incrementais muito pequenas com breves pausas de medição até apontar o caminho certo. Existem outras maneiras de lidar com isso?

sensors compass

— ViennaMike
fonte

O atraso na bússola é causado por um filtro passa-baixo, para suprimir ruídos de alta frequência.

Existem magnetômetros mais caros, com menos ruído e, portanto, menos lag.
Também é possível usar um giroscópio para melhorar a precisão. De fato, é isso que as Unidades de Medição Inercial (IMUs) fazem. Isso pode ser feito usando um filtro Kalman. Melhorar a precisão ajuda a diminuir o atraso, porque a precisão aumentada reduz a dependência de um filtro passa-baixo para suprimir o ruído. O filtro Kalman funde os dados do magnetômetro e também do giroscópio (que mede a taxa de mudança de posição).

Se você ficar com sua bússola atual, há duas soluções possíveis (aviso, isso fica cada vez mais avançado, mas a opção 1 deve estar acessível para a maioria das pessoas sem muito trabalho).

Você pode tentar cancelar o filtro. Isso pode remover o atraso, mas também aumenta o ruído de alta frequência. Depois de fazer isso, você pode tentar controlar o robô com base na nova estimativa de título. Para fazer isso, você deve experimentar os parâmetros do filtro passa-baixo. Por exemplo, em um tempo discreto, você pode encontrar:

ondeé a posição estimada (saída bússola) no tempo,é o cabeçalho real (verdade fundamental) no tempo.
$\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$
Você pode encontrar os parâmetros de fazendo um experimento onde você mede a verdade solo usando outros meios externos. Dado amostras, tem a seguinte $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]$
E você pode resolver encontrando: em queé a matriz de pseudo-inverso de. Não há uma maneira definitiva de calcular
$[\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$ , então você provavelmente irá adivinhar. Para pontos de bônus, isso pressupõe que o ruído seja branco e independente, mas você pode branquear primeiro para remover o viés e, portanto, melhorar sua estimativa dos parâmetros.
Você pode converter isso em uma função de transferência (também conhecida como transformação Z no domínio do 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}}$
Convertendo de volta para o domínio do 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$
A solução acima ainda não é a melhor maneira. A estimativa barulhenta pode não ser muito útil. Se colocarmos isso em uma equação de espaço de estado, podemos projetar um filtro Kalman e um controlador de feedback de estado completo usando LQR (regulador quadrático linear). A combinação de um filtro Kalman e um controlador LQR também é conhecida como um controlador LQG (gaussiano quadrático linear) e usa a recuperação de transferência de loop para obter um bom controlador.

Para fazer isso, crie as equações de espaço de estado (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)$
where $\vec{u}(t-1)$ represents the power in the motors to turn the robot, and $A_0$ , $A_1$ , $B_0$ , $B_1$ is how much it affects the heading based on position and speed (you can choose non-zero values for the other elements of the $B$ matrix, and first row of the $A$ matrix too).

Then, you can build your observer (Kalman Filter), by choosing noise estimates $Q_o$ and $R_o$ for the process noise and measurement noise. The Kalman Filter can then find the optimal estimate of heading, given those assumptions about the noise. After choosing the noise estimates, the implementation just depends on implementing code for the Kalman Filter (equations can be found on Wikipedia, so I won't go over it here).

After that, you can design an LQR controller, this time, choosing $Q_c$ and $R_c$ representing the weighting given to regulating the heading, and trying to limit the use of the actuators. In this case, you might choose $Q_c = \left[\matrix{1&0&0&\cdots&0\\0&0&0&\cdots&0\\\vdots&\vdots&\vdots&&\vdots\\0&0&0&\cdots&0}\right]$ and $R_c = \left[1\right]$ . This is done because LQR finds the optimal controller to minimise a cost function: $J = \sum{(\vec{x}^T Q\vec{x} + \vec{u}^T R \vec{u})}$

Then, you just put it through the discrete time algebraic Riccati equation:

$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$
and solve for a positive definite matrix $P$ .

Thus, your control law can be given by:

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

Finally, just doing this won't work very well, and is likely to be unstable because of the noise. Indeed, that means option 1 probably won't work unless you first put $\bar\theta$ through a low-pass filter (albeit not necessarily with such a long effective lag time). This is because while LQR is guaranteed stable, as soon as you use a Kalman filter, the guarantee is lost.

To fix this, we use the Loop Transfer Recovery technique, where you adjust the Kalman filter, and instead choose a new $Q_o = Q_0 + q^2BVB^T$ , where $Q_0$ is your original $Q$ matrix, tuned so that the Kalman filter is optimal. $V$ is any positive definite symmetric matrix, which you can just choose as the identity matrix ( $V=I$ ). Then, just choose a scalar $q$ . The resulting controller should become (more) stable as $q \rightarrow \infty$ , although the $Q_o$ matrix becomes de-tuned, which means it becomes less optimal.

Therefore, you just increase $q$ until it is stable. Another way you can try to make it stable, is to increase $R_c$ (or decrease $Q_c$ ) to make the LQR controller slower.

The concepts in this post does get quite advanced, but if you need to solve things like the Riccati equation, you can use MATLAB or other software to do this. There may also be libraries already implementing the Kalman filter (again, I believe MATLAB also does this).

For an embedded application, you may need to implement the Kalman filter yourself, although there is probably a C implementation.

— ronalchn
fonte

Thanks for you excellent and in-depth answer. I follow the gist of your first solution and I'm sure I can work through it. The second one, as you say, is more challenging and I'll have to work to see if I can follow it all.

— ViennaMike

A gyro is the simple answer. I've always heard, gyro for the short measurements, compass for the long. And realistically a cup of kallman filter between the two most of the time. The price of a 6DOF gyro/acc board is less than $20 these days, far too cheap to not use one.

At one time, I worked through someone else's Kallman filter. and got it working. A kallman filter is actually more of an approach, not a exact implementation, and in the gyro case, the end result does not need to use matrix math. It makes for much simpler code.

— Spiked3
fonte