处理指南针滞后（速率相关的磁滞）的方法有哪些？

12

我有一个踏板驱动的机器人，它具有用于跟踪距离的低精度车轮编码器和用于确定航向的电子罗盘。指南针在机器人快速转动时（例如到达航路点后）有明显的滞后时间（> 1秒），即在原地旋转以指向其新的航向。

有什么方法可以处理滞后现象？我认为可以进行大量测量并为指南针响应建模。但是，这似乎是有问题的，因为它取决于速率，并且我不知道瞬时速率。

作为一种简单但缓慢的方法，我让机器人转动直到它大致正确地指向正确的方向，然后通过短暂的测量暂停进行非常小的增量旋转，直到其指向正确的方向。还有其他解决方法吗？

sensors compass

— 维也纳迈克
source

12

指南针的滞后是由于要抑制高频噪声的低通滤波器。

存在更昂贵的磁力计，其具有更少的噪声，因此更少的滞后。
也可以使用陀螺仪来提高精度。实际上，这就是惯性测量单位（IMU）的作用。这可以通过使用卡尔曼滤波器来实现。精度的提高有助于减少滞后，因为提高的精度会降低对低通滤波器抑制噪声的依赖性。卡尔曼滤波器融合了磁力计和陀螺仪（用于测量航向变化率）中的数据。

如果您坚持使用当前的指南针，则有两种可能的解决方案（警告，这确实会越来越先进，但是大多数人无需太多工作即可使用选项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（线性二次调节器）的全状态反馈控制器。卡尔曼滤波器和LQR控制器的组合也称为LQG控制器（线性二次高斯），并使用环路转移恢复来获得良好的控制器。

为此，提出（离散）状态空间方程式：

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

然后，您可以通过为过程噪声和测量噪声选择噪声估计和来构建观察者（卡尔曼滤波器）。给定关于噪声的那些假设，然后卡尔曼滤波器可以找到航向的最佳估计。在选择了噪声估计之后，实现仅取决于实现卡尔曼滤波器的代码（可以在Wikipedia上找到等式，因此在这里不再赘述）。 $Q_o$ $R_o$

之后，您可以设计一个LQR控制器，这一次，选择和代表调节航向的权重，并尝试限制执行器的使用。在这种情况下，您可以选择和。之所以这样做，是因为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})}$

然后，您将其通过离散时间代数Riccati方程进行求解：

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

最后，仅执行此操作将无法很好地进行，并且可能会由于噪音而变得不稳定。确实，这意味着选项1可能无法工作，除非您首先将通过低通滤波器（尽管不一定需要这么长的有效滞后时间）。这是因为虽然可以保证LQR稳定，但是一旦您使用卡尔曼滤波器，就会失去保证。 $\bar\theta$

为了解决这个问题，我们使用“循环转移恢复”技术，您可以在其中调整卡尔曼滤波器，而是选择一个新的，其中是您的原始矩阵，已调整为使卡尔曼滤波器最佳。是任何正定对称矩阵，您可以选择它作为恒等矩阵（）。然后，只需选择一个标量。尽管矩阵，结果控制器应该变得更稳定，如，这意味着它变得不太理想。 $Q_o = Q_0 + q^2BVB^T$ $Q_0$ $Q$ $V$ $V=I$ $q$ $q \rightarrow \infty$ $Q_o$

因此，只需增加直到稳定。可以尝试使其稳定的另一种方法是增加（或减小）以使LQR控制器变慢。 $q$ $R_c$ $Q_c$

这篇文章中的概念确实非常先进，但是如果您需要解决Riccati方程之类的问题，则可以使用MATLAB或其他软件来完成。可能已经有一些库已经实现了Kalman过滤器（再次，我相信MATLAB也可以做到这一点）。

对于嵌入式应用程序，尽管可能存在C实现，但是您可能需要自己实现Kalman过滤器。

— 罗纳尔奇恩
source

感谢您的出色而深入的回答。我遵循您的第一个解决方案的要旨，我确信我可以解决它。正如您所说，第二个更具挑战性，我将不得不努力研究我是否可以完全遵循。

— ViennaMike

4

陀螺仪是简单的答案。我一直听说，陀螺仪用于短测量，指南针用于长测量。实际上，在大多数情况下，两次之间都需要一杯卡尔曼过滤器。如今，一块6DOF陀螺/ acc板的价格不到20美元，太便宜了，根本不使用它。

一次，我通过别人的Kallman过滤器工作。并使其工作。实际上，卡尔曼滤波器更像是一种方法，而不是确切的实现，并且在陀螺仪的情况下，最终结果不需要使用矩阵数学。它使代码更简单。

— 尖刺3
source