通过传感器融合从运动跟踪装置的输出计算偏航、俯仰和横滚【源码】

传感器融合根据运动跟踪设备的输出来计算航向,俯仰和横滚。这使用了Madgwick算法,该算法因其速度和质量而在多直升机设计中广泛使用。Pyboard上的更新时间不到2mS。最初的Madgwick研究表明,10-50Hz的更新速率足以获得准确的结果,这表明此实现的性能足够快。

提供了两种实现:一种用于同步代码,一种用于基于的异步应用程序asyncio。后者提供角度数据的连续背景更新,从而以最小的延迟进行访问。

本文档介绍了在运行MicroPython的单个平台上获取传感器数据并执行融合的情况。

支持其他模式:

  • 融合和数据采集在标准Python上的通用设备上运行。
  • 融合和数据采集在通过某种形式的通信链接链接的独立设备上运行。在这种模式下,数据获取设备可以运行任何类型的代码并以任何格式返回数据,而用户应用程序则将数据读取并将其转换为库可接受的形式。

MicroPython问题

该代码旨在独立于传感器设备:使用InvenSense MPU-9150进行了测试。

该算法广泛使用浮点数学。在MicroPython下,这意味着RAM分配。尽管代码被设计为平台无关的,但是在具有少量RAM的平台上可能会遇到问题。选项是使用冻结的字节码并定期运行垃圾回收。后者即使在Pyboard上也是可取的。有关fusionlcd.py示例,请参见测试程序。

MicroPython固件依赖性

该库中的某些模块使用异步编程。它使用asyncioCPythonuasyncio下MicroPython下的 库。完全重写后,MicroPython版本已得到很大改进,并且版本为3.0。它与CPython 3.8语法兼容asyncio。所有代码已更新为使用此语法,较早的版本不支持此语法。

要运行异步模块,MicroPython目标应该使用每日构建的固件,或者在V1.12之后使用发行版:此类固件包含uasyncio V3。

在CPython下运行异步代码的地方,该代码必须为V3.8或更高版本。

角度

带有和不带有磁力计的惯性测量单元(IMU)。带有它们的传感器称为9DOF,不带有它们的传感器称为6DOF传感器,其中DOF代表“自由度”。6DOF传感器无法提供航向信息,因为该信息是参考地球磁场得出的。

传感器融合的目的是确定车辆相对于地球的姿态。这从以下三个角度表达:

  1. heading相对于北方的角度。请注意,某些资料来源使用术语“偏航”。由于这也用来表示飞机机身相对于其运动方向的角度,因此我避免了它。
  2. pitch飞机机头相对于地面的角度(通常+ ve朝向地面)。也称为“海拔”。
  3. roll 飞机机翼到地面的角度,也称为“倾斜”。

在此实施方式中,这些以度为单位。

感测器

传感器驱动程序提供的测量单位仅在陀螺仪的情况下才重要。这必须提供每秒度数的度量。在该算法中对来自加速度计(通常为g)和磁力计(通常为Microtesla)的值进行了归一化。这利用了这些矢量的大小局部恒定的事实。因此,测量单位无关紧要。

模块

  1. fusion.py 标准同步融合库。
  2. fusion_async.py 使用uasyncio的库版本,可无障碍地访问俯仰,航向和横滚。
  3. deltat.py 控制上述时间。
  4. orientate.py 用于调整IMU方向以进行传感器融合的实用程序。

测试/演示程序:

  1. fusiontest.py 一个简单的同步库测试程序。
  2. fusiontest6.py 以上是6DOF传感器的变体。
  3. fusiontest_as.py 异步库的简单测试。
  4. fusiontest_as6.py 以上是6DOF传感器的变体。
  5. fusionlcd.py 使用Hitachi HD44780 2行LCD文本显示器测试异步库,以连续显示角度值。

源代码获取请阅读全文

隐藏内容 需要支付:¥10

未经允许不得转载!通过传感器融合从运动跟踪装置的输出计算偏航、俯仰和横滚【源码】