Arduino和Python卡尔曼滤波对四元数进行姿态测定

在本文中，我将演示使用EKF（扩展卡尔曼滤波）对四元数确定姿态的实现，并说明将多个传感器数据融合在一起以使系统正常工作的必要性。

将要使用的传感器是陀螺仪，加速度计和磁力计。 Arduino用于从传感器读取数据，但是数据处理将在python中完成。 除此之外，我还使用Pygame创建了一个简单的显示器，以便更好地可视化结果。 我还将提供一种校准磁力计的简单方法，因为它们的读数会根据周围环境而变化很大。

绘制图形

在本节中，我们将创建可视化工具，以便了解问题并更直观地得出结果。

Python完整代码

注意：BoardDisplay代码引用了Wireframe代码，因此您需要将包含Wireframe.py的文件夹标记为源根。 您可以通过右键单击Pycharm中的文件夹，选择“将目录标记为”，然后选择“源根目录”来执行此操作。 如果您这样做正确，则该文件夹应标记为蓝色。

创建线框

为了在Pygame中显示对象，我们首先需要完全定义其坐标。 为简单起见，我将显示一个矩形块来模仿我正在使用的板，因此我们要做的就是定义该块的6个面。 我们将分3个步骤进行操作。 首先，我们将有一个名为“节点”的类来定义点的xyz坐标，然后我们将有另一个名为“ Face”的类，它使用4个“节点”来定义矩形块的面。 然后，我们将有最后一个名为“ Wireframe”的类，它存储了多维数据集的信息，并且还充当了Pygame显示代码和后端计算代码之间的接口。

当我们开始谈论四元数时，后端计算部分将出现在第3部分中，因此，现在，我们仅专注于在Pygame中显示块。下面是上述3个类的代码。

代码略

我还创建了一些方便的功能来记录日志。 这有助于确保我们将信息正确插入到类中。 我们将在主要的“ BoardDisplay”代码中使用此类。 这是一个示例，说明如何使用该类初始化块的参数。

代码略

如果运行上述代码，则应打印出所有已创建的节点和面。如果您完全使用了我上面写的内容，那么您应该得到以下输出：

-- Nodes --
0: (-1.50, -1.00, -0.10) Color: (255, 255, 255)
1: (-1.50, -1.00, 0.10) Color: (255, 255, 255)
2: (-1.50, 1.00, -0.10) Color: (255, 255, 255)
3: (-1.50, 1.00, 0.10) Color: (255, 255, 255)
4: (1.50, -1.00, -0.10) Color: (255, 255, 255)
5: (1.50, -1.00, 0.10) Color: (255, 255, 255)
6: (1.50, 1.00, -0.10) Color: (255, 255, 255)
7: (1.50, 1.00, 0.10) Color: (255, 255, 255)

-- Faces --
Face 0:
Color: (255, 0, 255)
Node 0
Node 2
Node 6
Node 4
Face 1:
...
...
...

我只复制了面0的打印输出，但是由于我们总共插入了6张面的信息，因此应该可以打印到面 5为止的打印输出。 让我们首先从节点开始。 我们已经插入了8个节点的信息，并且您可以在打印输出中看到，它们的索引从0到7。对于该项目，颜色信息是多余的，因为我们将显示每个面的颜色而不是每个节点的颜色。但是，节点的索引非常重要，因为面对象引用此信息。 例如，我将面0指定为节点0、2、6和4所包围的面。下图显示了我使用的索引的顺序。 程序段的中心位于轴的原点。

这样，我们现在有了一个完全定义的块。下一步是创建在Pygame顶部看到的视图。

创建透视图

您的计算机屏幕上将显示一个二维图像。 应该没有深度，因为它只是一块平坦的屏幕。 那么，为什么上面的图在我们眼中看起来像是3D图？ 这实际上是由于构成对象（以及轴）的线条给人以深度的错觉。 这是一篇有关深度感知的文章，尽管我真的很有趣，但是如果您有兴趣的话，可以随时阅读。 在计算机上创建“深度”并不难，但是如果您不使用透视图（例如正交视图）绘制对象，即使它实际上是正确的，它也会在我们眼中变形。

在本节中，我们仅讨论一个消失点透视图的简单实现。下面是我要为查看器创建的透视图。

首先，我要使原点成为消失点，因此我将不得不将对象移出原点。 为此，我将在z方向上将对象的中心偏移“ P”，以便如图所示，对象的新中心现在位于（0，0，P）。 接下来，我要将“屏幕”放在z轴上的距离“ S”处。 该屏幕是二维的Pygame屏幕。 我们现在需要做的是将立方体上的所有8个点投影到屏幕上。 如果仔细观察投影，您会发现这只是x和y坐标的线性缩放，基于与屏幕的距离。

让我们以坐标为（-1.50，-1.00，-0.10）的节点0为例。 当我们将对象移出原点时，此节点的新坐标将为（-1.50，-1.00，-0.10 P）。 为了将此点投影到屏幕上，我们可以基于相似的三角形对其进行缩放。 当z = 0时，我们知道该点消失了，因此x和y都变为零。 这是我们计算的参考点。 如果屏幕位于z = S = 5，那么我们新的投影x坐标将为：

而我们新的投影y坐标将是：

投影的深度要复杂一些，因此在此不再赘述，因为这不是本文的主题。

上面的方程式是在我的Python代码中实现的。以下是代码摘录：

代码略

现在我们准备将点绘制到我们的Pygame屏幕上，让我们进入下一部分。

Pygame中绘图

为了显示对象，我使用了Pygame的绘制多边形方法。 如果您想检阅此文档，请参阅此处的文档。 基本上，我们可以使用pygame.draw.polygon方法为多边形提供点列表，以绘制多边形。 由于我们的块是一个简单的六边形矩形，因此我们必须用一个包含4个点的列表来调用此函数，总共需要6次，每个面总共一次。 但是，不可能显示块的所有6面，因为在任何时候，我们只能看到3个面。 那么我们如何确定哪些面可见？ 我们可以使用简单的画家算法来实现。

绘制图景时，我们通常从最远的图景开始绘画，然后逐渐在这些图景上绘制更近的对象。 这也是画家算法的作用。 我们要做的就是根据其深度（在面的中心）对6个面进行排序，然后从最远的面开始绘制多边形。 这在代码中实现如下：

代码略

首先，我们确定面中心的深度（面节点的平均深度），然后按递增顺序对其进行排序并按顺序绘制它们。

现在，您的块是固定的，除了绿色矩形之外，您什么都看不到：

图像略

轮换

我将介绍一些四元数的基础知识，并提供使用python的简单实现以可视化轮换。 但是，我们不会从第一原理中得出任何推论，因为它是完全数学的。 相反，我们将使用现有公式来构建代码。

四元数

四元数为我们提供了一种绕指定轴轮换指定角度的方法。 如果您只是刚开始3D轮换这一主题，那么您会经常听到人们说“使用四元数，因为它会产生万向节锁定问题”。 的确如此，但是轮换矩阵也是如此。 轮换矩阵不会遇到万向架锁定问题。 实际上，完全没有意义。 当使用欧拉角时，会发生万向架锁定问题，欧拉角只是一组3个基本轮换，以允许您描述3D空间中的任何方向。 在姿态确定中，我们通常将3D轮换可视化为偏航，俯仰和滚动的组合。 这些是欧拉角，因此无论您是否使用四元数，它们都容易受到云台锁定问题的影响。

四元数之所以这样命名是因为总共有4个分量。如果q是四元数，则

您可以将四元数视为复数的扩展，其中现在有1个实数和3个虚数，而不是1个实数和1个虚数。表示四元数的另一种方法是这样的：

但是，请注意，有些作者在虚部之前先写虚部。例如，

通常，您可以通过检查哪些变量为粗体（或斜体）来判断它们使用的顺序。 标记的变量通常是虚部，未标记的部分（通常带有数字下标）是实部。 在本文中，我们将使用第一个定义，其中实部在前。

四元数的共轭

定义如下。

不用担心它有什么用，因为您很快就会在下面找到。

四元数的大小以类似于矢量的方式定义。

因此，可以按以下方式定义单元四元数。

其中

下面是四元数加法和减法的定义（只需将“ +”替换为“-”）

详情参阅 - 亚图跨际