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

#1 楼

罗盘中的滞后是由于要抑制高频噪声的低通滤波器。


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

如果坚持使用当前的指南针，有两种可能的解决方案（警告，这样做变得越来越先进，但是大多数人无需太多工作就可以使用选项1。）



您可以尝试取消过滤器。这样可以消除滞后，但也会增加高频噪声。完成此操作后，您可以尝试根据新的航向估计值控制机器人。为此，您必须尝试计算出低通滤波器参数。例如，在离散时间内，您可能会发现：

$$ \ hat \ theta（t）= a_0 \ theta（t）+ a_1 \ theta（t-1）+ \ cdots + a_k \ theta（tk）$$
其中$ \ hat \ theta（t）$是时间$ t $的估计航向（罗盘输出），$ \ theta $是时间$ t的实际航向（地面真相） $。

您可以通过进行实验来找到参数$ a_i $，在该实验中，您可以使用其他一些外部方法来测量基本事实。给定$ n + k + 1 $个样本，您具有以下等式：
$$ \ 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] $$

您可以通过以下方法解决：
$$ \ 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 \ Theta（z）} {\ Theta（z）} = a_0 + a_1 z ^ {-1 } + ... + a_k z ^ {-k} $$

要取消此设置，我们可以取反函数（其中$ \ bar \ theta（t）$是我们新的航向估计）：

$$ \ frac {\ bar \ Theta（z）} {\ hat \ Theta（z）} = \ frac {1} {a_0 + a_1 z ^ {-1} + \ cdots + a_k z ^ {-k}} $$

转换回时域：

$$ a_0 \ bar \ theta（t）+ a_1 \ bar \ theta（t-1）+ \ cdots + a_k \ bar \ theta（tk）= \ hat \ theta（t）$$

$$ \ bar \ theta（t）= \ frac {\ hat \ theta（t）-a_1 \ bar \ theta（t-1）-\ cdots-a_k \ bar \ theta（tk）} {a_0} $$

然后我们可以使用$ \ bar \ theta $来控制机器人。

这会非常嘈杂，因此您可能仍要在使用前将$ \ bar \ theta $通过低通滤波器放置（尽管也许s的延迟较小）。


上述解决方案仍然不是最佳方法。嘈杂的估计可能不是很有用。如果将其放入状态空间方程中，则可以设计一个卡尔曼滤波器，以及使用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）= \ left [\ matrix {\ theta（t）\\\ theta（t-1） \\\ cdots \\\ theta（tk）} \ 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）= \ 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 $来构建观察者（卡尔曼滤波器）过程噪声和测量噪声。给定关于噪声的那些假设，然后卡尔曼滤波器可以找到航向的最佳估计。选择了噪声估计后，实现方式仅取决于实现卡尔曼滤波器的代码（可以在Wikipedia上找到等式，因此在这里不再赘述）。

之后，您可以设计一个LQR控制器，这一次，选择$ Q_c $和$ R_c $代表调节航向的权重，并尝试限制执行器的使用。在这种情况下，您可以选择$ Q_c = \ left [\ matrix {1＆0＆0＆\ cdots＆0 \\ 0＆0＆0＆0＆cdots＆0 \\\ vdots＆\ vdots＆\ vdots && \\ vdots \\ 0＆0＆0＆\ cdots＆0} \ right] $和$ R_c = \ left [1 \ right] $。这样做是因为LQR找到了一个最优控制器来最小化成本函数：$ J = \ sum {（\ vec {x} ^ TQ \ vec {x} + \ vec {u} ^ TR \ vec {u}）} $

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

$$ P = Q + A ^ T \ left（P-PB \ left（R + B ^ TPB \ right）^ {-1} B ^ TP \ right）A $$

并求解正定矩阵$ P $。

因此，您的控制定律可以通过以下方式给出：

$$ \ vec {u}（t）=-K（\ vec {x}（t）-\ vec {x} _ {ref}（t） ）$$

其中$ K =（R + B ^ TPB）^ {-1}（B ^ TPA）$

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

要解决此问题，我们使用了环路传输恢复技术，您可以在其中调整卡尔曼滤波器，而是选择一个新的$ Q_o = Q_0 + q ^ 2BVB ^ T $，其中$ Q_0 $是您原始的$ Q $矩阵，对其进行了调整，以使Kalman滤波器达到最佳状态。 $ V $是任何正定对称矩阵，您可以选择将其作为单位矩阵（$ V = I $）。然后，只需选择一个标量$ q $。尽管$ Q_o $矩阵失调了，但最终得到的控制器应该随着$ q \ rightarrow \ infty $变得（更）稳定，这意味着它变得不那么理想。

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


这篇文章中的概念确实很实用。进阶，但是如果您需要求解Riccati方程式，可以使用MATLAB或其他软件来完成。

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

#2 楼

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

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