Arduino和Python卡尔曼滤波对四元数进行姿态测定 二维码
45
关联知识在本文中，我将演示使用EKF（扩展卡尔曼滤波）对四元数确定姿态的实现，并说明将多个传感器数据融合在一起以使系统正常工作的必要性。 将要使用的传感器是陀螺仪，加速度计和磁力计。 Arduino用于从传感器读取数据，但是数据处理将在python中完成。 除此之外，我还使用Pygame创建了一个简单的显示器，以便更好地可视化结果。 我还将提供一种校准磁力计的简单方法，因为它们的读数会根据周围环境而变化很大。 绘制图形在本节中，我们将创建可视化工具，以便了解问题并更直观地得出结果。 Python完整代码注意：BoardDisplay代码引用了Wireframe代码，因此您需要将包含Wireframe.py的文件夹标记为源根。 您可以通过右键单击Pycharm中的文件夹，选择“将目录标记为”，然后选择“源根目录”来执行此操作。 如果您这样做正确，则该文件夹应标记为蓝色。 创建线框为了在Pygame中显示对象，我们首先需要完全定义其坐标。 为简单起见，我将显示一个矩形块来模仿我正在使用的板，因此我们要做的就是定义该块的6个面。 我们将分3个步骤进行操作。 首先，我们将有一个名为“节点”的类来定义点的xyz坐标，然后我们将有另一个名为“ Face”的类，它使用4个“节点”来定义矩形块的面。 然后，我们将有最后一个名为“ Wireframe”的类，它存储了多维数据集的信息，并且还充当了Pygame显示代码和后端计算代码之间的接口。 当我们开始谈论四元数时，后端计算部分将出现在第3部分中，因此，现在，我们仅专注于在Pygame中显示块。下面是上述3个类的代码。 代码略 我还创建了一些方便的功能来记录日志。 这有助于确保我们将信息正确插入到类中。 我们将在主要的“ BoardDisplay”代码中使用此类。 这是一个示例，说明如何使用该类初始化块的参数。 代码略 如果运行上述代码，则应打印出所有已创建的节点和面。如果您完全使用了我上面写的内容，那么您应该得到以下输出：
我只复制了面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个虚数。表示四元数的另一种方法是这样的： 但是，请注意，有些作者在虚部之前先写虚部。例如， 通常，您可以通过检查哪些变量为粗体（或斜体）来判断它们使用的顺序。 标记的变量通常是虚部，未标记的部分（通常带有数字下标）是实部。 在本文中，我们将使用第一个定义，其中实部在前。 四元数的共轭定义如下。 不用担心它有什么用，因为您很快就会在下面找到。 四元数的大小以类似于矢量的方式定义。 因此，可以按以下方式定义单元四元数。 其中表示单位四元数。 下面是四元数加法和减法的定义（只需将“ +”替换为“-”） 接下来是虚部的定义 这样，您就可以进行四元数乘法。这与将4变量代数与另一个4变量代数相乘的方法相同。推导变得很长，所以我将跳过它，只发布答案。 上述乘法也可以矩阵乘法的形式编写，如下所示： 现在，我们终于可以谈论轮换了。如果我们以以下方式定义四元数： 然后， 鉴于矢量和关于矢量的角度，因此是轮换矢量，以上内容可以再次写成矩阵乘法而不是四元数乘法。 数学很乏味，所以我将再次发布结果。 其中C为 这种矩阵形式很重要，因为它使我们能够与从欧拉角得出的轮换矩阵进行比较，以确定物体的姿态（偏航，俯仰，横滚）。 最后是所有最令人兴奋的方程式。 其中为 ，，分别是x，y和z方向上的角速度。 该方程式为我们提供了一种直接使用陀螺仪中的值将其转换为轮换的方法！ 在矩阵中工作更直观，因此我们将上面的内容转换为矩阵。 请注意，此处可以用2种不同的方式表示相同的方程式。 其中和为 现在，让我们在之前创建的Pygame矩形块中测试一下轮换方法。 测试轮换BoardDisplay代码引用线框代码，而Wireframe代码引用四元数代码。 为了让Pycharm知道在哪里可以找到所有相关文件，您需要将包含所有文件的文件夹标记为源根。 您可以通过右键单击Pycharm中的文件夹，选择“将目录标记为”，然后选择“源根目录”来执行此操作。 如果您这样做正确，则该文件夹在Pycharm中应标记为蓝色。 本部分的目的是将角速度转换为对象的旋转。 为了进行测试，我们将使用恒定的角速度作为输入，并观察对象在Pygame中的旋转方式。 我们可以从上面使用等式（1）和（2）来实现，但是它们是连续的时间形式，因此我们必须先将其离散化。 为了离散方程（2），我们将采用一阶泰勒级数变换。可以非常简单地完成此操作，如下所示： 其中是样本k + 1和k之间的时间。 方程（2）中的其他形式也可以相同。这样做将产生以下结果： 您可以在四元数代码中看到，这实际上是我在轮换方法中实现的方程式。
现在，我在理解q实际表示时遇到了一些问题，因为有时q可以采用向量的形式（标量部分为0）。 在此，q表示将对象的原始方向与对象的当前方向映射的四元数。 因此，当我将q初始化为[1、0、0、0]时，实际上是在暗示对象的起始方向是Pygame中对象的方向。 现在我们还没有真正要测试的物理对象，因此听起来有些怪异，但是一旦进入下一部分，它应该变得更加清晰。 这只是意味着现实生活中的对象与Pygame在时间0处显示的对象处于相同的方向。 确定欧拉角接下来，我们需要一种确定欧拉角的方法，因为它们处理起来更直观。 等式（1）为我们提供了使用四元数的旋转矩阵，但实际上我们也可以使用欧拉角确定相同的旋转矩阵（万向节锁定期间遇到的奇异点除外）。 我们将使用“偏航，俯仰，横滚”约定，因此请注意，如果使用其他顺序，则会得到不同的结果。 正偏航定义为绕z轴逆时针旋转。偏航角为α的旋转矩阵为： 正螺距定义为绕y轴逆时针旋转。螺距角为β的旋转矩阵为： 正横摇定义为绕x轴逆时针旋转。横滚角为的旋转矩阵为： 任何旋转都可以分解为这3个基本旋转，因此横滚，俯仰，偏航旋转（请注意此处的旋转顺序）可以由以下矩阵描述： 请注意，以上等式仅适用于首先执行横摇，俯仰然后最后偏航的旋转。 如果更改顺序，则最终将得到不同的旋转矩阵。 现在，如果四元数轮换矩阵是正确的，那当然是因为世界上有很多人使用了它，那么我们可以使用上面的滚动，俯仰，偏航旋转矩阵从四元数中获得相应的欧拉角轮换矩阵。 根据等式（1），让矩阵C中的每个单元格都有一个索引： 内容略 利用MEMS陀螺仪数据实现旋转（+代码）校准磁力计错误模型测量模型校准备扩展卡尔曼滤波实现（+代码）卡尔曼滤波状态加速计数据磁力计数据四元数EKF实现 |
